diff --git a/bash_scripts/start_control_pc.sh b/bash_scripts/start_control_pc.sh index 4007812..7e480a7 100755 --- a/bash_scripts/start_control_pc.sh +++ b/bash_scripts/start_control_pc.sh @@ -37,7 +37,7 @@ old_gripper=0 log_on_franka_interface=0 stop_on_error=0 -while getopts ':h:i:u:p:d:r:s:g:o:l:e' option; do +while getopts ':h:i:u:p:d:r:a:s:g:o:l:e' option; do case "${option}" in h) echo "$usage" exit diff --git a/frankapy/franka_arm.py b/frankapy/franka_arm.py index bc85eee..89cfdcc 100644 --- a/frankapy/franka_arm.py +++ b/frankapy/franka_arm.py @@ -89,7 +89,6 @@ def __init__( self._with_gripper = with_gripper self._old_gripper = old_gripper self._last_gripper_command = None - # init ROS if init_node: rospy.init_node(rosnode_name, @@ -97,7 +96,7 @@ def __init__( log_level=ros_log_level) self._collision_boxes_pub = BoxesPublisher('franka_collision_boxes_{}'.format(robot_num)) self._joint_state_pub = rospy.Publisher('franka_virtual_joints_{}'.format(robot_num), JointState, queue_size=10) - + self._state_client = FrankaArmStateClient( new_ros_node=False, robot_state_server_name=self._robot_state_server_name, @@ -115,7 +114,6 @@ def __init__( self._execute_skill_action_server_name, ExecuteSkillAction) self._client.wait_for_server() self.wait_for_franka_interface() - if self._with_gripper and not self._old_gripper: self._gripper_homing_client = SimpleActionClient( self._gripper_homing_action_server_name, HomingAction) @@ -171,6 +169,7 @@ def wait_for_franka_interface(self, timeout=None): if franka_interface_status.is_ready: return sleep(1e-2) + raise FrankaArmCommException('FrankaInterface status not ready for {}s'.format( FC.DEFAULT_FRANKA_INTERFACE_TIMEOUT)) @@ -550,6 +549,40 @@ def goto_pose_delta(self, block=block, ignore_errors=ignore_errors) + def apply_joint_torques(self, + desired_torque, + duration=5, + buffer_time=FC.DEFAULT_TERM_BUFFER_TIME, + skill_desc='ApplyJointTorque' + ): + """ + Commands Arm to apply desired joint torque + desired_torque : :obj:`list` + A list of 7 numbers that correspond to joint joint torques in Nm. + duration : :obj:`float` + How much time this robot motion should take. + buffer_time : :obj:`float` + How much extra time the termination handler will wait + before stopping the skill after duration has passed. + skill_desc : :obj:`str` + Skill description to use for logging on the Control PC. + """ + skill = Skill(SkillType.JointTorqueSkill, + TrajectoryGeneratorType.MinJerkJointTrajectoryGenerator, + feedback_controller_type=FeedbackControllerType.TorqueFeedbackController, + termination_handler_type=TerminationHandlerType.TimeTerminationHandler, + skill_desc=skill_desc) + skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) + skill.add_time_termination_params(buffer_time) + skill.add_run_time(duration) + skill.add_goal_joints(duration, desired_torque) + goal = skill.create_goal() + self._send_goal(goal, + cb=lambda x: skill.feedback_callback(x), + block=False, + ignore_errors=False) + sleep(FC.DYNAMIC_SKILL_WAIT_TIME) + def goto_joints(self, joints, @@ -2092,24 +2125,6 @@ def is_joints_reachable(self, joints): """ Unimplemented """ - - def apply_joint_torques(self, torques, duration, ignore_errors=True, skill_desc='',): - """ - NOT IMPLEMENTED YET - - Commands the arm to apply given joint torques for duration seconds. - - Parameters - ---------- - torques : :obj:`list` - A list of 7 numbers that correspond to torques in Nm. - duration : :obj:`float` - How much time this robot motion should take. - skill_desc : :obj:`str` - Skill description to use for logging on control-pc. - """ - pass - def set_speed(self, speed): """ NOT IMPLEMENTED YET diff --git a/frankapy/franka_interface_common_definitions.py b/frankapy/franka_interface_common_definitions.py index 9bceb8c..5fad3b3 100644 --- a/frankapy/franka_interface_common_definitions.py +++ b/frankapy/franka_interface_common_definitions.py @@ -23,6 +23,7 @@ class SkillType: GripperSkill = _enum_auto('SkillType') ImpedanceControlSkill = _enum_auto('SkillType') JointPositionSkill = _enum_auto('SkillType') + JointTorqueSkill = _enum_auto('SkillType') class MetaSkillType: @@ -64,7 +65,7 @@ class FeedbackControllerType: NoopFeedbackController = _enum_auto('FeedbackControllerType') PassThroughFeedbackController = _enum_auto('FeedbackControllerType') SetInternalImpedanceFeedbackController = _enum_auto('FeedbackControllerType') - + TorqueFeedbackController = _enum_auto('FeedbackControllerType') class TerminationHandlerType: ContactTerminationHandler = _enum_auto('TerminationHandlerType') @@ -91,3 +92,4 @@ class SensorDataMessageType: POSE_POSITION_VELOCITY = _enum_auto('SensorDataMessageType') POSE_POSITION = _enum_auto('SensorDataMessageType') SHOULD_TERMINATE = _enum_auto('SensorDataMessageType') + JOINT_TORQUE = _enum_auto('SensorDataMessageType') diff --git a/frankapy/proto/sensor_msg.proto b/frankapy/proto/sensor_msg.proto index ec79797..438e847 100644 --- a/frankapy/proto/sensor_msg.proto +++ b/frankapy/proto/sensor_msg.proto @@ -78,3 +78,19 @@ message ForcePositionControllerSensorMessage { repeated double force_kps_joint = 6; repeated double selection = 7; } + +message JointImpedanceSensorMessage { + required int32 id = 1; + required double timestamp = 2; + + repeated double joint_stiffnesses = 3; + +} + +message TorqueControllerSensorMessage{ + required int32 id = 1; + required double timestamp = 2; + + repeated double joint_torques_cmd = 3; +} +