From 55e211599cdc8b352b7be31c6f18f794430e2656 Mon Sep 17 00:00:00 2001 From: Kevin Zhang Date: Fri, 13 Sep 2024 14:54:38 -0400 Subject: [PATCH] Added removing gravity. --- frankapy/franka_arm.py | 8 ++++++-- frankapy/proto/feedback_controller_params_msg.proto | 3 ++- frankapy/proto/feedback_controller_params_msg_pb2.py | 4 ++-- frankapy/proto/sensor_msg.proto | 3 ++- frankapy/proto/sensor_msg_pb2.py | 6 +++--- frankapy/skill_list.py | 6 ++++-- 6 files changed, 19 insertions(+), 11 deletions(-) diff --git a/frankapy/franka_arm.py b/frankapy/franka_arm.py index e6e362e..0608d72 100644 --- a/frankapy/franka_arm.py +++ b/frankapy/franka_arm.py @@ -867,7 +867,8 @@ def execute_joint_velocities(self, def execute_joint_torques(self, joint_torques, - selection, + selection=[0,0,0,0,0,0,0], + remove_gravity=[0,0,0,0,0,0,0], duration=5, dynamic=True, buffer_time=FC.DEFAULT_TERM_BUFFER_TIME, @@ -887,6 +888,9 @@ def execute_joint_torques(self, selection : :obj:`list` A list of 7 numbers that indicate whether to use the joint torques passed in or not. If 1 is passed in for a specific joint, the robot will use the joint torque for that joint. + remove_gravity : :obj:`list` + A list of 7 numbers that indicate whether to remove gravity from that joint or not. + If 1 is passed in for a specific joint, the robot will subtract gravity for that joint. duration : :obj:`float` How much time this robot motion should take. dynamic : :obj:`bool` @@ -941,7 +945,7 @@ def execute_joint_torques(self, else: skill.add_joint_threshold_params(buffer_time, FC.DEFAULT_JOINT_THRESHOLDS) - skill.add_joint_torques(joint_torques, selection) + skill.add_joint_torques(joint_torques, selection, remove_gravity) goal = skill.create_goal() diff --git a/frankapy/proto/feedback_controller_params_msg.proto b/frankapy/proto/feedback_controller_params_msg.proto index 5e409af..a01d860 100644 --- a/frankapy/proto/feedback_controller_params_msg.proto +++ b/frankapy/proto/feedback_controller_params_msg.proto @@ -33,5 +33,6 @@ message ForcePositionFeedbackControllerMessage { message JointTorqueFeedbackControllerMessage { repeated double selection = 1; - required double joint_torques = 2; + repeated double remove_gravity = 2; + repeated double joint_torques = 3; } diff --git a/frankapy/proto/feedback_controller_params_msg_pb2.py b/frankapy/proto/feedback_controller_params_msg_pb2.py index e871068..c996c66 100644 --- a/frankapy/proto/feedback_controller_params_msg_pb2.py +++ b/frankapy/proto/feedback_controller_params_msg_pb2.py @@ -13,7 +13,7 @@ -DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n$feedback_controller_params_msg.proto\"p\n+CartesianImpedanceFeedbackControllerMessage\x12!\n\x19translational_stiffnesses\x18\x01 \x03(\x01\x12\x1e\n\x16rotational_stiffnesses\x18\x02 \x03(\x01\"q\n\"ForceAxisFeedbackControllerMessage\x12\x1f\n\x17translational_stiffness\x18\x01 \x02(\x01\x12\x1c\n\x14rotational_stiffness\x18\x02 \x02(\x01\x12\x0c\n\x04\x61xis\x18\x03 \x03(\x01\"K\n\'JointImpedanceFeedbackControllerMessage\x12\x0f\n\x07k_gains\x18\x01 \x03(\x01\x12\x0f\n\x07\x64_gains\x18\x02 \x03(\x01\"d\n*InternalImpedanceFeedbackControllerMessage\x12\x1c\n\x14\x63\x61rtesian_impedances\x18\x01 \x03(\x01\x12\x18\n\x10joint_impedances\x18\x02 \x03(\x01\"\xc0\x01\n&ForcePositionFeedbackControllerMessage\x12\x19\n\x11position_kps_cart\x18\x01 \x03(\x01\x12\x16\n\x0e\x66orce_kps_cart\x18\x02 \x03(\x01\x12\x1a\n\x12position_kps_joint\x18\x03 \x03(\x01\x12\x17\n\x0f\x66orce_kps_joint\x18\x04 \x03(\x01\x12\x11\n\tselection\x18\x05 \x03(\x01\x12\x1b\n\x13use_cartesian_gains\x18\x06 \x02(\x08\"P\n$JointTorqueFeedbackControllerMessage\x12\x11\n\tselection\x18\x01 \x03(\x01\x12\x15\n\rjoint_torques\x18\x02 \x02(\x01') +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n$feedback_controller_params_msg.proto\"p\n+CartesianImpedanceFeedbackControllerMessage\x12!\n\x19translational_stiffnesses\x18\x01 \x03(\x01\x12\x1e\n\x16rotational_stiffnesses\x18\x02 \x03(\x01\"q\n\"ForceAxisFeedbackControllerMessage\x12\x1f\n\x17translational_stiffness\x18\x01 \x02(\x01\x12\x1c\n\x14rotational_stiffness\x18\x02 \x02(\x01\x12\x0c\n\x04\x61xis\x18\x03 \x03(\x01\"K\n\'JointImpedanceFeedbackControllerMessage\x12\x0f\n\x07k_gains\x18\x01 \x03(\x01\x12\x0f\n\x07\x64_gains\x18\x02 \x03(\x01\"d\n*InternalImpedanceFeedbackControllerMessage\x12\x1c\n\x14\x63\x61rtesian_impedances\x18\x01 \x03(\x01\x12\x18\n\x10joint_impedances\x18\x02 \x03(\x01\"\xc0\x01\n&ForcePositionFeedbackControllerMessage\x12\x19\n\x11position_kps_cart\x18\x01 \x03(\x01\x12\x16\n\x0e\x66orce_kps_cart\x18\x02 \x03(\x01\x12\x1a\n\x12position_kps_joint\x18\x03 \x03(\x01\x12\x17\n\x0f\x66orce_kps_joint\x18\x04 \x03(\x01\x12\x11\n\tselection\x18\x05 \x03(\x01\x12\x1b\n\x13use_cartesian_gains\x18\x06 \x02(\x08\"h\n$JointTorqueFeedbackControllerMessage\x12\x11\n\tselection\x18\x01 \x03(\x01\x12\x16\n\x0eremove_gravity\x18\x02 \x03(\x01\x12\x15\n\rjoint_torques\x18\x03 \x03(\x01') _builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) _builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'feedback_controller_params_msg_pb2', globals()) @@ -31,5 +31,5 @@ _FORCEPOSITIONFEEDBACKCONTROLLERMESSAGE._serialized_start=449 _FORCEPOSITIONFEEDBACKCONTROLLERMESSAGE._serialized_end=641 _JOINTTORQUEFEEDBACKCONTROLLERMESSAGE._serialized_start=643 - _JOINTTORQUEFEEDBACKCONTROLLERMESSAGE._serialized_end=723 + _JOINTTORQUEFEEDBACKCONTROLLERMESSAGE._serialized_end=747 # @@protoc_insertion_point(module_scope) diff --git a/frankapy/proto/sensor_msg.proto b/frankapy/proto/sensor_msg.proto index 3bc6b41..0a045a9 100644 --- a/frankapy/proto/sensor_msg.proto +++ b/frankapy/proto/sensor_msg.proto @@ -98,5 +98,6 @@ message JointTorqueControllerSensorMessage { required double timestamp = 2; repeated double selection = 3; - repeated double joint_torques = 4; + repeated double remove_gravity = 4; + repeated double joint_torques = 5; } \ No newline at end of file diff --git a/frankapy/proto/sensor_msg_pb2.py b/frankapy/proto/sensor_msg_pb2.py index 2de99d4..342fc2e 100644 --- a/frankapy/proto/sensor_msg_pb2.py +++ b/frankapy/proto/sensor_msg_pb2.py @@ -13,7 +13,7 @@ -DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x10sensor_msg.proto\"S\n\x0b\x42oundingBox\x12\x0c\n\x04name\x18\x01 \x02(\t\x12\n\n\x02id\x18\x02 \x02(\x05\x12\t\n\x01x\x18\x03 \x02(\x05\x12\t\n\x01y\x18\x04 \x02(\x05\x12\t\n\x01w\x18\x05 \x02(\x05\x12\t\n\x01h\x18\x06 \x02(\x05\"}\n\"JointPositionVelocitySensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x14\n\x0cseg_run_time\x18\x03 \x02(\x01\x12\x0e\n\x06joints\x18\x04 \x03(\x01\x12\x12\n\njoint_vels\x18\x05 \x03(\x01\"\x97\x01\n!PosePositionVelocitySensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x14\n\x0cseg_run_time\x18\x03 \x02(\x01\x12\x10\n\x08position\x18\x04 \x03(\x01\x12\x12\n\nquaternion\x18\x05 \x03(\x01\x12\x17\n\x0fpose_velocities\x18\x06 \x03(\x01\"U\n\x1aJointVelocitySensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x18\n\x10joint_velocities\x18\x03 \x03(\x01\"K\n\x1aJointPositionSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x0e\n\x06joints\x18\x03 \x03(\x01\"`\n\x19PosePositionSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x10\n\x08position\x18\x03 \x03(\x01\x12\x12\n\nquaternion\x18\x04 \x03(\x01\"]\n\x1e\x43\x61rtesianVelocitySensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x1c\n\x14\x63\x61rtesian_velocities\x18\x03 \x03(\x01\"K\n\x1cShouldTerminateSensorMessage\x12\x11\n\ttimestamp\x18\x01 \x02(\x01\x12\x18\n\x10should_terminate\x18\x02 \x02(\x08\"\x83\x01\n\x1f\x43\x61rtesianImpedanceSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12!\n\x19translational_stiffnesses\x18\x03 \x03(\x01\x12\x1e\n\x16rotational_stiffnesses\x18\x04 \x03(\x01\"n\n\x1a\x46orcePositionSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x14\n\x0cseg_run_time\x18\x03 \x02(\x01\x12\x0c\n\x04pose\x18\x04 \x03(\x01\x12\r\n\x05\x66orce\x18\x05 \x03(\x01\"\xc0\x01\n$ForcePositionControllerSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x19\n\x11position_kps_cart\x18\x03 \x03(\x01\x12\x16\n\x0e\x66orce_kps_cart\x18\x04 \x03(\x01\x12\x1a\n\x12position_kps_joint\x18\x05 \x03(\x01\x12\x17\n\x0f\x66orce_kps_joint\x18\x06 \x03(\x01\x12\x11\n\tselection\x18\x07 \x03(\x01\"m\n\"JointTorqueControllerSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x11\n\tselection\x18\x03 \x03(\x01\x12\x15\n\rjoint_torques\x18\x04 \x03(\x01') +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x10sensor_msg.proto\"S\n\x0b\x42oundingBox\x12\x0c\n\x04name\x18\x01 \x02(\t\x12\n\n\x02id\x18\x02 \x02(\x05\x12\t\n\x01x\x18\x03 \x02(\x05\x12\t\n\x01y\x18\x04 \x02(\x05\x12\t\n\x01w\x18\x05 \x02(\x05\x12\t\n\x01h\x18\x06 \x02(\x05\"}\n\"JointPositionVelocitySensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x14\n\x0cseg_run_time\x18\x03 \x02(\x01\x12\x0e\n\x06joints\x18\x04 \x03(\x01\x12\x12\n\njoint_vels\x18\x05 \x03(\x01\"\x97\x01\n!PosePositionVelocitySensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x14\n\x0cseg_run_time\x18\x03 \x02(\x01\x12\x10\n\x08position\x18\x04 \x03(\x01\x12\x12\n\nquaternion\x18\x05 \x03(\x01\x12\x17\n\x0fpose_velocities\x18\x06 \x03(\x01\"U\n\x1aJointVelocitySensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x18\n\x10joint_velocities\x18\x03 \x03(\x01\"K\n\x1aJointPositionSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x0e\n\x06joints\x18\x03 \x03(\x01\"`\n\x19PosePositionSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x10\n\x08position\x18\x03 \x03(\x01\x12\x12\n\nquaternion\x18\x04 \x03(\x01\"]\n\x1e\x43\x61rtesianVelocitySensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x1c\n\x14\x63\x61rtesian_velocities\x18\x03 \x03(\x01\"K\n\x1cShouldTerminateSensorMessage\x12\x11\n\ttimestamp\x18\x01 \x02(\x01\x12\x18\n\x10should_terminate\x18\x02 \x02(\x08\"\x83\x01\n\x1f\x43\x61rtesianImpedanceSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12!\n\x19translational_stiffnesses\x18\x03 \x03(\x01\x12\x1e\n\x16rotational_stiffnesses\x18\x04 \x03(\x01\"n\n\x1a\x46orcePositionSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x14\n\x0cseg_run_time\x18\x03 \x02(\x01\x12\x0c\n\x04pose\x18\x04 \x03(\x01\x12\r\n\x05\x66orce\x18\x05 \x03(\x01\"\xc0\x01\n$ForcePositionControllerSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x19\n\x11position_kps_cart\x18\x03 \x03(\x01\x12\x16\n\x0e\x66orce_kps_cart\x18\x04 \x03(\x01\x12\x1a\n\x12position_kps_joint\x18\x05 \x03(\x01\x12\x17\n\x0f\x66orce_kps_joint\x18\x06 \x03(\x01\x12\x11\n\tselection\x18\x07 \x03(\x01\"\x85\x01\n\"JointTorqueControllerSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x11\n\tselection\x18\x03 \x03(\x01\x12\x16\n\x0eremove_gravity\x18\x04 \x03(\x01\x12\x15\n\rjoint_torques\x18\x05 \x03(\x01') _builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) _builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'sensor_msg_pb2', globals()) @@ -42,6 +42,6 @@ _FORCEPOSITIONSENSORMESSAGE._serialized_end=1064 _FORCEPOSITIONCONTROLLERSENSORMESSAGE._serialized_start=1067 _FORCEPOSITIONCONTROLLERSENSORMESSAGE._serialized_end=1259 - _JOINTTORQUECONTROLLERSENSORMESSAGE._serialized_start=1261 - _JOINTTORQUECONTROLLERSENSORMESSAGE._serialized_end=1370 + _JOINTTORQUECONTROLLERSENSORMESSAGE._serialized_start=1262 + _JOINTTORQUECONTROLLERSENSORMESSAGE._serialized_end=1395 # @@protoc_insertion_point(module_scope) diff --git a/frankapy/skill_list.py b/frankapy/skill_list.py index fdbee4a..3655b84 100644 --- a/frankapy/skill_list.py +++ b/frankapy/skill_list.py @@ -158,16 +158,18 @@ def add_internal_impedances(self, cartesian_impedances, joint_impedances): self.add_feedback_controller_params(internal_feedback_controller_msg_proto.SerializeToString()) - def add_joint_torques(self, joint_torques, selection): + def add_joint_torques(self, joint_torques, selection, remove_gravity): assert type(joint_torques) is list, "Incorrect joint_torques type. Should be list." assert type(selection) is list, "Incorrect selection type. Should be list." + assert type(remove_gravity) is list, "Incorrect remove_gravity type. Should be list." assert len(joint_torques) == 7, "Incorrect joint_torques len. Should be 7." assert len(selection) == 7, "Incorrect selection len. Should be 7." + assert len(remove_gravity) == 7, "Incorrect remove_gravity len. Should be 7." assert self._skill_type == SkillType.ImpedanceControlSkill, \ "Incorrect skill type. Should be ImpedanceControlSkill" joint_torque_controller_msg_proto = \ - JointTorqueFeedbackControllerMessage(joint_torques=joint_torques, selection=selection) + JointTorqueFeedbackControllerMessage(joint_torques=joint_torques, selection=selection, remove_gravity=remove_gravity) self.add_torque_controller_params(joint_torque_controller_msg_proto.SerializeToString())