class FrankaArm:
def __init__(self,
rosnode_name='franka_arm_client',
ros_log_level=rospy.INFO,
robot_num=1):
self._execute_skill_action_server_name = \
'/execute_skill_action_server_node_'+str(robot_num)+'/execute_skill'
self._robot_state_server_name = \
'/get_current_robot_state_server_node_'+str(robot_num)+'/get_current_robot_state_server'
self._robolib_status_server_name = \
'/get_current_robolib_status_server_node_'+str(robot_num)+'/get_current_robolib_status_server'
self._connected = False
self._in_skill = False
# set signal handler to handle ctrl+c and kill sigs
signal.signal(signal.SIGINT, self._sigint_handler_gen())
# init ROS
rospy.init_node(rosnode_name,
disable_signals=True,
log_level=ros_log_level)
rospy.wait_for_service(self._robolib_status_server_name)
self._get_current_robolib_status = rospy.ServiceProxy(
self._robolib_status_server_name, GetCurrentRobolibStatusCmd)
self._state_client = FrankaArmStateClient(
new_ros_node=False,
robot_state_server_name=self._robot_state_server_name)
self._client = actionlib.SimpleActionClient(
self._execute_skill_action_server_name, ExecuteSkillAction)
self._client.wait_for_server()
self.wait_for_robolib()
# done init ROS
self._connected = True
# set default identity tool delta pose
self._tool_delta_pose = RigidTransform(from_frame='franka_tool',
to_frame='franka_tool_base')
def wait_for_robolib(self, timeout=None):
'''Blocks execution until robolib gives ready signal.
'''
timeout = FC.DEFAULT_ROBOLIB_TIMEOUT if timeout is None else timeout
t_start = time()
while time() - t_start < timeout:
robolib_status = self._get_current_robolib_status().robolib_status
if robolib_status.is_ready:
return
sleep(1e-2)
raise FrankaArmCommException('Robolib status not ready for {}s'.format(
FC.DEFAULT_ROBOLIB_TIMEOUT))
def _sigint_handler_gen(self):
def sigint_handler(sig, frame):
if self._connected and self._in_skill:
self._client.cancel_goal()
sys.exit(0)
return sigint_handler
def _send_goal(self, goal, cb, ignore_errors=True):
'''
Raises:
FrankaArmCommException if a timeout is reached
FrankaArmException if robolib gives an error
FrankaArmRobolibNotReadyException if robolib is not ready
'''
self._in_skill = True
self._client.send_goal(goal, feedback_cb=cb)
done = False
while not done:
robolib_status = self._get_current_robolib_status().robolib_status
e = None
if rospy.is_shutdown():
e = RuntimeError('rospy is down!')
elif robolib_status.error_description:
e = FrankaArmException(robolib_status.error_description)
elif not robolib_status.is_ready:
e = FrankaArmRobolibNotReadyException()
if e is not None:
if ignore_errors:
self.wait_for_robolib()
break
else:
raise e
done = self._client.wait_for_result(
rospy.Duration.from_sec(FC.ACTION_WAIT_LOOP_TIME))
self._in_skill = False
return self._client.get_result()
'''
Controls
'''
def goto_pose(self,
tool_pose,
duration=3,
stop_on_contact_forces=None,
cartesian_impedances=None,
ignore_errors=True,
skill_desc='',
skill_type=SkillType.ImpedanceControlSkill):
'''Commands Arm to the given pose via linear interpolation
Args:
tool_pose (RigidTransform) : End-effector pose in tool frame
duration (float) : How much time this robot motion should take
stop_on_contact_forces (list): List of 6 floats corresponding to
force limits on translation (xyz) and rotation about (xyz) axes.
Default is None. If None then will not stop on contact.
ignore_errors : function ignores errors by default. If False, errors
and some exceptions can be thrown
skill_desc (string) : Skill description to use for logging on
control-pc.
'''
return self._goto_pose(tool_pose, duration, stop_on_contact_forces,
cartesian_impedances, ignore_errors, skill_desc,
skill_type)
def goto_pose_with_cartesian_control(self,
tool_pose,
duration=3.,
stop_on_contact_forces=None,
cartesian_impedances=None,
ignore_errors=True,
skill_desc=''):
'''Commands Arm to the given pose via min-jerk interpolation.
Use Franka's internal Cartesian Controller to execute this skill.
Args:
tool_pose (RigidTransform) : End-effector pose in tool frame
duration (float) : How much time this robot motion should take
stop_on_contact_forces (list): List of 6 floats corresponding to
force limits on translation (xyz) and rotation about (xyz) axes.
Default is None. If None then will not stop on contact.
ignore_errors : function ignores errors by default. If False, errors
and some exceptions can be thrown
skill_desc (string) : Skill description to use for logging on
control-pc.
'''
return self._goto_pose(tool_pose,
duration,
stop_on_contact_forces,
cartesian_impedances,
ignore_errors,
skill_desc,
skill_type=SkillType.CartesianPoseSkill)
def goto_pose_with_impedance_control(self,
tool_pose,
duration=3.,
stop_on_contact_forces=None,
cartesian_impedances=None,
ignore_errors=True,
skill_desc=''):
'''Commands Arm to the given pose via min-jerk interpolation.
Use our own impedance controller (use jacobian to convert cartesian
poses to joints.)
Args:
tool_pose (RigidTransform) : End-effector pose in tool frame
duration (float) : How much time this robot motion should take
stop_on_contact_forces (list): List of 6 floats corresponding to
force limits on translation (xyz) and rotation about (xyz) axes.
Default is None. If None then will not stop on contact.
ignore_errors : function ignores errors by default. If False, errors
and some exceptions can be thrown
skill_desc (string) : Skill description to use for logging on
control-pc.
'''
return self._goto_pose(tool_pose,
duration,
stop_on_contact_forces,
cartesian_impedances,
ignore_errors,
skill_desc,
skill_type=SkillType.ImpedanceControlSkill)
def _goto_pose(self,
tool_pose,
duration=3,
stop_on_contact_forces=None,
cartesian_impedances=None,
ignore_errors=True,
skill_desc='',
skill_type=None):
'''Commands Arm to the given pose via linear interpolation
Args:
tool_pose (RigidTransform) : End-effector pose in tool frame
duration (float) : How much time this robot motion should take
stop_on_contact_forces (list): List of 6 floats corresponding to
force limits on translation (xyz) and rotation about (xyz) axes.
Default is None. If None then will not stop on contact.
'''
if tool_pose.from_frame != 'franka_tool' or tool_pose.to_frame != 'world':
raise ValueError(
'pose has invalid frame names! Make sure pose has \
from_frame=franka_tool and to_frame=world')
tool_base_pose = tool_pose * self._tool_delta_pose.inverse()
skill = GoToPoseSkill(skill_desc, skill_type)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
if cartesian_impedances is not None:
skill.add_cartesian_impedances(cartesian_impedances)
else:
skill.add_feedback_controller_params(
FC.DEFAULT_TORQUE_CONTROLLER_PARAMS)
if stop_on_contact_forces is not None:
skill.add_contact_termination_params(FC.DEFAULT_TERM_BUFFER_TIME,
stop_on_contact_forces,
stop_on_contact_forces)
else:
skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME])
skill.add_goal_pose_with_matrix(
duration,
tool_base_pose.matrix.T.flatten().tolist())
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
ignore_errors=ignore_errors)
def goto_pose_delta(self,
delta_tool_pose,
duration=3,
stop_on_contact_forces=None,
cartesian_impedances=None,
ignore_errors=True,
skill_desc='',
skill_type=SkillType.ImpedanceControlSkill):
'''Commands Arm to the given delta pose via linear interpolation and
uses impedance control.
Args:
delta_tool_pose (RigidTransform) : Delta pose in tool frame
duration (float) : How much time this robot motion should take
stop_on_contact_forces (list): List of 6 floats corresponding to
force limits on translation (xyz) and rotation about (xyz) axes.
Default is None. If None then will not stop on contact.
ignore_errors : function ignores errors by default. If False, errors
and some exceptions can be thrown
skill_desc (string) : Skill description to use for logging on
control-pc.
'''
return self._goto_pose_delta(delta_tool_pose, duration,
stop_on_contact_forces,
cartesian_impedances, ignore_errors,
skill_desc, skill_type)
def goto_pose_delta_with_impedance_control(self,
delta_tool_pose,
duration=3,
stop_on_contact_forces=None,
cartesian_impedances=None,
ignore_errors=True,
skill_desc=''):
return self._goto_pose_delta(
delta_tool_pose,
duration,
stop_on_contact_forces,
cartesian_impedances,
ignore_errors,
skill_desc,
skill_type=SkillType.ImpedanceControlSkill)
def goto_pose_delta_with_cartesian_control(self,
delta_tool_pose,
duration=3,
stop_on_contact_forces=None,
cartesian_impedances=None,
ignore_errors=True,
skill_desc=''):
'''Commands Arm to the given delta pose via linear interpolation using
franka's internal cartesian control.
Args:
delta_tool_pose (RigidTransform) : Delta pose in tool frame
duration (float) : How much time this robot motion should take
stop_on_contact_forces (list): List of 6 floats corresponding to
force limits on translation (xyz) and rotation about (xyz) axes.
Default is None. If None then will not stop on contact.
cartesian_impedance (list): List of 6 floats. Used to set the cartesian
impedance of Franka's internal cartesian controller.
List of (x, y, z, roll, pitch, yaw)
ignore_errors : function ignores errors by default. If False, errors
and some exceptions can be thrown
skill_desc (string) : Skill description to use for logging on
control-pc.
'''
return self._goto_pose_delta(delta_tool_pose,
duration,
stop_on_contact_forces,
cartesian_impedances,
ignore_errors,
skill_desc,
skill_type=SkillType.CartesianPoseSkill)
def _goto_pose_delta(self,
delta_tool_pose,
duration=3,
stop_on_contact_forces=None,
cartesian_impedances=None,
ignore_errors=True,
skill_desc='',
skill_type=SkillType.ImpedanceControlSkill):
'''Commands Arm to the given delta pose via linear interpolation
Args:
delta_tool_pose (RigidTransform) : Delta pose in tool frame
duration (float) : How much time this robot motion should take
stop_on_contact_forces (list): List of 6 floats corresponding to
force limits on translation (xyz) and rotation about (xyz) axes.
Default is None. If None then will not stop on contact.
'''
if delta_tool_pose.from_frame != 'franka_tool' \
or delta_tool_pose.to_frame != 'franka_tool':
raise ValueError('delta_pose has invalid frame names! ' \
'Make sure delta_pose has from_frame=franka_tool ' \
'and to_frame=franka_tool')
delta_tool_base_pose = self._tool_delta_pose \
* delta_tool_pose * self._tool_delta_pose.inverse()
skill = GoToPoseDeltaSkill(skill_desc, skill_type)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
if cartesian_impedances is not None:
skill.add_cartesian_impedances(cartesian_impedances)
else:
skill.add_feedback_controller_params(
FC.DEFAULT_TORQUE_CONTROLLER_PARAMS)
if stop_on_contact_forces is not None:
skill.add_contact_termination_params(FC.DEFAULT_TERM_BUFFER_TIME,
stop_on_contact_forces,
stop_on_contact_forces)
else:
skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME])
skill.add_relative_motion_with_quaternion(
duration, delta_tool_base_pose.translation.tolist(),
delta_tool_base_pose.quaternion.tolist())
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
ignore_errors=ignore_errors)
def goto_joints(self,
joints,
duration=5,
stop_on_contact_forces=None,
joint_impedances=None,
k_gains=None,
d_gains=None,
ignore_errors=True,
skill_desc='',
skill_type=SkillType.JointPositionSkill):
'''Commands Arm to the given joint configuration
Args:
joints (list): A list of 7 numbers that correspond to joint angles
in radians
duration (float): How much time this robot motion should take
joint_impedances (list): A list of 7 numbers that represent the desired
joint impedances for the internal robot joint
controller
Raises:
ValueError: If is_joints_reachable(joints) returns False
'''
return self._goto_joints(joints, duration, stop_on_contact_forces,
joint_impedances, k_gains, d_gains,
ignore_errors, skill_desc, skill_type)
def goto_joints_with_joint_control(self,
joints,
duration=5,
stop_on_contact_forces=None,
joint_impedances=None,
ignore_errors=True,
skill_desc=''):
'''Commands Arm to the given joint configuration
Args:
joints (list): A list of 7 numbers that correspond to joint angles
in radians
duration (float): How much time this robot motion should take
joint_impedances (list): A list of 7 numbers that represent the desired
joint impedances for the internal robot joint
controller
Raises:
ValueError: If is_joints_reachable(joints) returns False
'''
return self._goto_joints(joints,
duration,
stop_on_contact_forces,
joint_impedances,
None,
None,
ignore_errors,
skill_desc,
skill_type=SkillType.JointPositionSkill)
def goto_joints_with_impedance_control(self,
joints,
duration=5,
stop_on_contact_forces=None,
k_gains=None,
d_gains=None,
ignore_errors=True,
skill_desc=''):
'''Commands Arm to the given joint configuration
Args:
joints (list): A list of 7 numbers that correspond to joint angles
in radians
duration (float): How much time this robot motion should take
joint_impedances (list): A list of 7 numbers that represent the desired
joint impedances for the internal robot joint
controller
Raises:
ValueError: If is_joints_reachable(joints) returns False
'''
return self._goto_joints(joints,
duration,
stop_on_contact_forces,
None,
k_gains,
d_gains,
ignore_errors,
skill_desc,
skill_type=SkillType.ImpedanceControlSkill)
def _goto_joints(self,
joints,
duration=5,
stop_on_contact_forces=None,
joint_impedances=None,
k_gains=None,
d_gains=None,
ignore_errors=True,
skill_desc='',
skill_type=SkillType.JointPositionSkill):
'''Commands Arm to the given joint configuration
Args:
joints (list): A list of 7 numbers that correspond to joint angles
in radians
duration (float): How much time this robot motion should take
joint_impedances (list): A list of 7 numbers that represent the desired
joint impedances for the internal robot joint
controller
Raises:
ValueError: If is_joints_reachable(joints) returns False
'''
if not self.is_joints_reachable(joints):
raise ValueError('Joints not reachable!')
skill = GoToJointsSkill(skill_desc, skill_type)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
if joint_impedances is not None:
skill.add_joint_impedances(joint_impedances)
elif k_gains is not None and d_gains is not None:
skill.add_joint_gains(k_gains, d_gains)
else:
if (skill_type == SkillType.ImpedanceControlSkill):
skill.add_joint_gains(FC.DEFAULT_K_GAINS, FC.DEFAULT_D_GAINS)
else:
skill.add_feedback_controller_params([])
if stop_on_contact_forces is not None:
skill.add_contact_termination_params(FC.DEFAULT_TERM_BUFFER_TIME,
stop_on_contact_forces,
stop_on_contact_forces)
else:
skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME])
skill.add_goal_joints(duration, joints)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
ignore_errors=ignore_errors)
def apply_joint_torques(self, torques, duration, ignore_errors=True):
'''Commands Arm to apply given joint torques for duration seconds
Args:
torques (list): A list of 7 numbers that correspond to torques in Nm.
duration (float): A float in the unit of seconds
'''
pass
def execute_joint_dmp(self,
dmp_info,
duration,
ignore_errors=True,
skill_desc='',
skill_type=SkillType.JointPositionSkill):
'''Commands Arm to execute a given dmp for duration seconds
Args:
dmp_info (dict): Contains all the parameters of a DMP
(phi_j, tau, alpha, beta, num_basis, num_sensors, mu, h,
and weights)
duration (float): A float in the unit of seconds
'''
skill = JointDMPSkill(skill_desc, skill_type)
skill.add_initial_sensor_values(dmp_info['phi_j']) # sensor values
# Doesn't matter because we overwrite it with the initial joint positions anyway
y0 = [-0.282, -0.189, 0.0668, -2.186, 0.0524, 1.916, -1.06273]
# Run time, tau, alpha, beta, num_basis, num_sensor_value, mu, h, weight
trajectory_params = [
duration, dmp_info['tau'], dmp_info['alpha'], dmp_info['beta'],
float(dmp_info['num_basis']), float(dmp_info['num_sensors'])] \
+ dmp_info['mu'] \
+ dmp_info['h'] \
+ y0 \
+ np.array(dmp_info['weights']).reshape(-1).tolist()
skill.add_trajectory_params(trajectory_params)
skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME])
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
ignore_errors=ignore_errors)
def execute_pose_dmp(self,
dmp_info,
duration,
ignore_errors=True,
skill_desc='',
skill_type=SkillType.CartesianPoseSkill):
'''Commands Arm to execute a given dmp for duration seconds
Args:
dmp_info (dict): Contains all the parameters of a DMP
(phi_j, tau, alpha, beta, num_basis, num_sensors, mu, h,
and weights)
duration (float): A float in the unit of seconds
'''
skill = PoseDMPSkill(skill_desc, skill_type)
skill.add_initial_sensor_values(dmp_info['phi_j']) # sensor values
# Doesn't matter because we overwrite it with the initial position anyways
y0 = [0.0, 0.0, 0.0]
# Run time, tau, alpha, beta, num_basis, num_sensor_value, mu, h, weight
trajectory_params = [
duration, dmp_info['tau'], dmp_info['alpha'], dmp_info['beta'],
float(dmp_info['num_basis']), float(dmp_info['num_sensors'])] \
+ dmp_info['mu'] \
+ dmp_info['h'] \
+ y0 \
+ np.array(dmp_info['weights']).reshape(-1).tolist()
skill.add_trajectory_params(trajectory_params)
skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME])
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
ignore_errors=ignore_errors)
def execute_goal_pose_dmp(self,
dmp_info,
duration,
ignore_errors=True,
phi_j=None,
skill_desc='',
skill_type=SkillType.CartesianPoseSkill):
'''Commands Arm to execute a given dmp for duration seconds
Args:
dmp_info (dict): Contains all the parameters of a DMP
(phi_j, tau, alpha, beta, num_basis, num_sensors, mu, h,
and weights)
duration (float): A float in the unit of seconds
'''
skill = GoalPoseDMPSkill(skill_desc, skill_type)
skill.add_initial_sensor_values(dmp_info['phi_j']) # sensor values
# Doesn't matter because we overwrite it with the initial position anyways
y0 = [0.0, 0.0, 0.0]
if phi_j is None:
phi_j = np.array([[-0.025, 1.], [0, 0.], [-0.05, 1.0]])
# Run time, tau, alpha, beta, num_basis, num_sensor_value, mu, h, weight
trajectory_params = [
duration, dmp_info['tau'], dmp_info['alpha'], dmp_info['beta'],
float(dmp_info['num_basis']), float(dmp_info['num_sensors'])] \
+ dmp_info['mu'] \
+ dmp_info['h'] \
+ y0 \
+ np.array(dmp_info['weights']).reshape(-1).tolist() \
+ np.array(phi_j).reshape(-1).tolist()
skill.add_trajectory_params(trajectory_params)
skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME])
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
ignore_errors=ignore_errors)
def apply_effector_forces_torques(self,
run_duration,
acc_duration,
max_translation,
max_rotation,
forces=None,
torques=None,
ignore_errors=True,
skill_desc=''):
'''Applies the given end-effector forces and torques in N and Nm
Args:
run_duration (float): A float in the unit of seconds
acc_duration (float): A float in the unit of seconds. How long to
acc/de-acc to achieve desired force.
forces (list): Optional (defaults to None).
A list of 3 numbers that correspond to end-effector forces in
3 directions
torques (list): Optional (defaults to None).
A list of 3 numbers that correspond to end-effector torques in
3 axes
Raises:
ValueError if acc_duration > 0.5*run_duration, or if forces are
too large
'''
if acc_duration > 0.5 * run_duration:
raise ValueError(
'acc_duration must be smaller than half of run_duration!')
forces = [0, 0, 0] if forces is None else np.array(forces).tolist()
torques = [0, 0, 0] if torques is None else np.array(torques).tolist()
if np.linalg.norm(forces) * run_duration > FC.MAX_LIN_MOMENTUM:
raise ValueError('Linear momentum magnitude exceeds safety '
'threshold of {}'.format(FC.MAX_LIN_MOMENTUM))
if np.linalg.norm(torques) * run_duration > FC.MAX_ANG_MOMENTUM:
raise ValueError('Angular momentum magnitude exceeds safety '
'threshold of {}'.format(FC.MAX_ANG_MOMENTUM))
skill = ForceTorqueSkill(skill_desc=skill_desc)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.add_termination_params([0.1])
skill.add_trajectory_params(
[run_duration, acc_duration, max_translation, max_rotation] +
forces + torques)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
ignore_errors=ignore_errors)
def apply_effector_forces_along_axis(self,
run_duration,
acc_duration,
max_translation,
forces,
ignore_errors=True,
skill_desc=''):
'''Applies the given end-effector forces and torques in N and Nm
Args:
run_duration (float): A float in the unit of seconds
acc_duration (float): A float in the unit of seconds.
How long to acc/de-acc to achieve desired force.
max_translation (float): Max translation before the robot
deaccelerates.
forces (list): Optional (defaults to None).
A list of 3 numbers that correspond to end-effector forces in
3 directions
Raises:
ValueError if acc_duration > 0.5*run_duration, or if forces are
too large
'''
if acc_duration > 0.5 * run_duration:
raise ValueError(
'acc_duration must be smaller than half of run_duration!')
if np.linalg.norm(
forces) * run_duration > FC.MAX_LIN_MOMENTUM_CONSTRAINED:
raise ValueError('Linear momentum magnitude exceeds safety ' \
'threshold of {}'.format(FC.MAX_LIN_MOMENTUM_CONSTRAINED))
forces = np.array(forces)
force_axis = forces / np.linalg.norm(forces)
skill = ForceAlongAxisSkill(skill_desc=skill_desc)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.add_termination_params([0.1])
skill.add_feedback_controller_params(
FC.DEFAULT_FORCE_AXIS_CONTROLLER_PARAMS + force_axis.tolist())
init_params = [run_duration, acc_duration, max_translation, 0]
skill.add_trajectory_params(init_params + forces.tolist() + [0, 0, 0])
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
ignore_errors=ignore_errors)
def goto_gripper(self, width, speed=0.04, force=None, ignore_errors=True):
'''Commands gripper to goto a certain width, applying up to the given
(default is max) force if needed
Args:
width (float): A float in the unit of meters
speed (float): Gripper operation speed in meters per sec
force (float): Max gripper force to apply in N. Default to None,
which gives acceptable force
Raises:
ValueError: If width is less than 0 or greater than TODO(jacky)
the maximum gripper opening
'''
skill = GripperSkill()
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
if force is not None:
skill.add_trajectory_params([width, speed, force])
else:
skill.add_trajectory_params([width, speed])
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
ignore_errors=ignore_errors)
# this is so the gripper state can be updated, which happens with a
# small lag
sleep(FC.GRIPPER_CMD_SLEEP_TIME)
def stay_in_position(self,
duration=3,
translational_stiffness=600,
rotational_stiffness=50,
k_gains=None,
d_gains=None,
cartesian_impedances=None,
joint_impedances=None,
ignore_errors=True,
skill_desc='',
skill_type=SkillType.ImpedanceControlSkill,
feedback_controller_type=FeedbackControllerType.
CartesianImpedanceFeedbackController):
'''Commands the Arm to stay in its current position with provided
translation and rotation stiffnesses
Args:
duration (float) : How much time the robot should stay in place in
seconds.
translational_stiffness (float): Translational stiffness factor used
in the torque controller.
Default is 600. A value of 0 will allow free translational
movement.
rotational_stiffness (float): Rotational stiffness factor used in
the torque controller.
Default is 50. A value of 0 will allow free rotational movement.
'''
skill = StayInInitialPoseSkill(skill_desc, skill_type,
feedback_controller_type)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
if skill_type == SkillType.ImpedanceControlSkill:
if feedback_controller_type == FeedbackControllerType.CartesianImpedanceFeedbackController:
if cartesian_impedances is not None:
skill.add_cartesian_impedances(cartesian_impedances)
else:
skill.add_feedback_controller_params(
[translational_stiffness] + [rotational_stiffness])
elif feedback_controller_type == FeedbackControllerType.JointImpedanceFeedbackController:
if k_gains is not None and d_gains is not None:
skill.add_joint_gains(k_gains, d_gains)
else:
skill.add_feedback_controller_params([])
else:
skill.add_feedback_controller_params(
[translational_stiffness] + [rotational_stiffness])
elif skill_type == SkillType.CartesianPoseSkill:
if cartesian_impedances is not None:
skill.add_cartesian_impedances(cartesian_impedances)
else:
skill.add_feedback_controller_params([])
elif skill_type == SkillType.JointPositionSkill:
if joint_impedances is not None:
skill.add_joint_impedances(joint_impedances)
else:
skill.add_feedback_controller_params([])
else:
skill.add_feedback_controller_params([translational_stiffness] +
[rotational_stiffness])
skill.add_run_time(duration)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
ignore_errors=ignore_errors)
def run_guide_mode_with_selective_joint_compliance(
self,
duration=3,
joint_impedances=None,
k_gains=FC.DEFAULT_K_GAINS,
d_gains=FC.DEFAULT_D_GAINS,
ignore_errors=True,
skill_desc='',
skill_type=SkillType.ImpedanceControlSkill):
'''Run guide mode with selective joint compliance given k and d gains
for each joint
Args:
duration (float) : How much time the robot should be in selective
joint guide mode in seconds.
k_gains (list): list of 7 k gains, one for each joint
Default is 600., 600., 600., 600., 250., 150., 50..
d_gains (list): list of 7 d gains, one for each joint
Default is 50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0.
'''
skill = StayInInitialPoseSkill(
skill_desc, skill_type,
FeedbackControllerType.JointImpedanceFeedbackController)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
if skill_type == SkillType.ImpedanceControlSkill:
if k_gains is not None and d_gains is not None:
skill.add_joint_gains(k_gains, d_gains)
elif skill_type == SkillType.JointPositionSkill:
if joint_impedances is not None:
skill.add_joint_impedances(joint_impedances)
else:
skill.add_feedback_controller_params([])
skill.add_run_time(duration)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
ignore_errors=ignore_errors)
def run_guide_mode_with_selective_pose_compliance(
self,
duration=3,
translational_stiffnesses=FC.DEFAULT_TRANSLATIONAL_STIFFNESSES,
rotational_stiffnesses=FC.DEFAULT_ROTATIONAL_STIFFNESSES,
cartesian_impedances=None,
ignore_errors=True,
skill_desc='',
skill_type=SkillType.ImpedanceControlSkill):
'''Run guide mode with selective pose compliance given translational
and rotational stiffnesses
Args:
duration (float) : How much time the robot should be in selective
pose guide mode in seconds.
translational_stiffnesses (list): list of 3 translational stiffnesses,
one for each axis (x,y,z) Default is 600.0, 600.0, 600.0
rotational_stiffnesses (list): list of 3 rotational stiffnesses,
one for axis (roll, pitch, yaw) Default is 50.0, 50.0, 50.0
'''
skill = StayInInitialPoseSkill(
skill_desc, skill_type,
FeedbackControllerType.CartesianImpedanceFeedbackController)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
if skill_type == SkillType.ImpedanceControlSkill:
if cartesian_impedances is not None:
skill.add_cartesian_impedances(cartesian_impedances)
else:
skill.add_feedback_controller_params(
[translational_stiffnesses] + [rotational_stiffnesses])
elif skill_type == SkillType.CartesianPoseSkill:
if cartesian_impedances is not None:
skill.add_cartesian_impedances(cartesian_impedances)
else:
skill.add_feedback_controller_params([])
skill.add_feedback_controller_params(translational_stiffnesses +
rotational_stiffnesses)
skill.add_run_time(duration)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
ignore_errors=ignore_errors)
def run_dynamic_joint_position_interpolation(
self,
joints,
duration=5,
stop_on_contact_forces=None,
joint_impedances=None,
k_gains=None,
d_gains=None,
ignore_errors=True,
skill_desc='',
skill_type=SkillType.JointPositionDynamicInterpolationSkill):
'''Commands Arm to the given joint configuration
Args:
joints (list): A list of 7 numbers that correspond to joint angles
in radians
duration (float): How much time this robot motion should take
joint_impedances (list): A list of 7 numbers that represent the desired
joint impedances for the internal robot joint
controller
Raises:
ValueError: If is_joints_reachable(joints) returns False
'''
if not self.is_joints_reachable(joints):
raise ValueError('Joints not reachable!')
skill = GoToJointsSkill(skill_desc, skill_type)
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
if joint_impedances is not None:
skill.add_joint_impedances(joint_impedances)
elif k_gains is not None and d_gains is not None:
skill.add_joint_gains(k_gains, d_gains)
else:
if (skill_type == SkillType.ImpedanceControlSkill):
skill.add_joint_gains(FC.DEFAULT_K_GAINS, FC.DEFAULT_D_GAINS)
else:
skill.add_feedback_controller_params([])
if stop_on_contact_forces is not None:
skill.add_contact_termination_params(FC.DEFAULT_TERM_BUFFER_TIME,
stop_on_contact_forces,
stop_on_contact_forces)
else:
skill.add_termination_params([FC.DEFAULT_TERM_BUFFER_TIME])
skill.add_goal_joints(duration, joints)
goal = skill.create_goal()
self._send_goal(goal,
cb=lambda x: skill.feedback_callback(x),
ignore_errors=ignore_errors)
def open_gripper(self):
'''Opens gripper to maximum width
'''
self.goto_gripper(FC.GRIPPER_WIDTH_MAX)
def close_gripper(self, grasp=True):
'''Closes the gripper as much as possible
'''
self.goto_gripper(FC.GRIPPER_WIDTH_MIN,
force=FC.GRIPPER_MAX_FORCE if grasp else None)
def run_guide_mode(self, duration=100):
self.apply_effector_forces_torques(duration, 0, 0, 0)
'''
Reads
'''
def get_robot_state(self):
'''
Returns:
dict of full robot state data
'''
return self._state_client.get_data()
def get_pose(self):
'''
Returns:
pose (RigidTransform) of the current end-effector
'''
tool_base_pose = self._state_client.get_pose()
tool_pose = tool_base_pose * self._tool_delta_pose
return tool_pose
def get_joints(self):
'''
Returns:
ndarray of shape (7,) of joint angles in radians
'''
return self._state_client.get_joints()
def get_joint_torques(self):
'''
Returns:
ndarray of shape (7,) of joint torques in Nm
'''
return self._state_client.get_joint_torques()
def get_joint_velocities(self):
'''
Returns:
ndarray of shape (7,) of joint velocities in rads/s
'''
return self._state_client.get_joint_velocities()
def get_gripper_width(self):
'''
Returns:
float of gripper width in meters
'''
return self._state_client.get_gripper_width()
def get_gripper_is_grasped(self):
'''
Returns:
True if gripper is grasping something. False otherwise
'''
return self._state_client.get_gripper_is_grasped()
def get_speed(self, speed):
'''
Returns:
float of current target speed parameter
'''
pass
def get_tool_base_pose(self):
'''
Returns:
RigidTransform of current tool base pose
'''
return self._tool_delta_pose.copy()
'''
Sets
'''
def set_tool_delta_pose(self, tool_delta_pose):
'''Sets the tool pose relative to the end-effector pose
Args:
tool_delta_pose (RigidTransform)
'''
if tool_delta_pose.from_frame != 'franka_tool' \
or tool_delta_pose.to_frame != 'franka_tool_base':
raise ValueError('tool_delta_pose has invalid frame names! ' \
'Make sure it has from_frame=franka_tool, and ' \
'to_frame=franka_tool_base')
self._tool_delta_pose = tool_delta_pose.copy()
def set_speed(self, speed):
'''Sets current target speed parameter
Args:
speed (float)
'''
pass
'''
Misc
'''
def reset_joints(self, skill_desc='', ignore_errors=True):
'''Commands Arm to goto hardcoded home joint configuration
'''
self.goto_joints(FC.HOME_JOINTS,
duration=5,
skill_desc=skill_desc,
ignore_errors=ignore_errors)
def reset_pose(self, skill_desc='', ignore_errors=True):
'''Commands Arm to goto hardcoded home pose
'''
self.goto_pose(FC.HOME_POSE,
duration=5,
skill_desc=skill_desc,
ignore_errors=ignore_errors)
def is_joints_reachable(self, joints):
'''
Returns:
True if all joints within joint limits
'''
for i, val in enumerate(joints):
if val <= FC.JOINT_LIMITS_MIN[i] or val >= FC.JOINT_LIMITS_MAX[i]:
return False
return True
class FrankaArm:
def __init__(self, rosnode_name='franka_arm_client'):
self._connected = False
self._in_skill = False
# set signal handler to handle ctrl+c and kill sigs
signal.signal(signal.SIGINT, self._sigint_handler_gen())
# init ROS
rospy.init_node(rosnode_name, disable_signals=True)
self._sub = FrankaArmSubscriber(new_ros_node=False)
self._client = actionlib.SimpleActionClient('/execute_skill_action_server_node/execute_skill', ExecuteSkillAction)
self._client.wait_for_server()
self._current_goal_handle = None
self._connected = True
# set default identity tool delta pose
self._tool_delta_pose = RigidTransform(from_frame='franka_tool', to_frame='franka_tool_base')
def _sigint_handler_gen(self):
def sigint_handler(sig, frame):
if self._connected and self._in_skill:
self._client.cancel_goal()
sys.exit(0)
return sigint_handler
def _send_goal(self, goal, cb):
'''
Raises:
FrankaArmCommException if a timeout is reached
'''
# TODO(jacky): institute a timeout check
self._client.send_goal(goal, feedback_cb=cb)
self._in_skill = True
self._client.wait_for_result()
self._in_skill = False
return self._client.get_result()
'''
Controls
'''
def goto_pose(self, tool_pose, duration=3):
'''Commands Arm to the given pose via linear interpolation
Args:
tool_pose (RigidTransform) : End-effector pose in tool frame
duration (float) : How much time this robot motion should take
Raises:
FrankaArmCollisionException if a collision is detected
'''
if pose.from_frame != 'franka_tool' or pose.to_frame != 'world':
raise ValueError('pose has invalid frame names! Make sure pose has from_frame=franka_tool and to_frame=world')
tool_base_pose = tool_pose * self._tool_delta_pose.inverse()
skill = ArmMoveToGoalWithDefaultSensorSkill()
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.add_feedback_controller_params(FC.DEFAULT_TORQUE_CONTROLLER_PARAMS)
skill.add_termination_params(FC.DEFAULT_TERM_PARAMS)
skill.add_trajectory_params([duration] + tool_base_pose.matrix.T.flatten().tolist())
goal = skill.create_goal()
self._send_goal(goal, cb=lambda x: skill.feedback_callback(x))
def goto_pose_delta(self, delta_tool_pose, duration=3):
'''Commands Arm to the given delta pose via linear interpolation
Args:
delta_tool_pose (RigidTransform) : Delta pose in tool frame
duration (float) : How much time this robot motion should take
Raises:
FrankaArmCollisionException if a collision is detected
'''
if delta_tool_pose.from_frame != 'franka_tool' or delta_tool_pose.to_frame != 'franka_tool':
raise ValueError('delta_pose has invalid frame names! Make sure delta_pose has from_frame=franka_tool and to_frame=franka_tool')
delta_tool_base_pose = self._tool_delta_pose * delta_tool_pose * self._tool_delta_pose.inverse()
skill = ArmRelativeMotionWithDefaultSensorSkill()
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.add_feedback_controller_params(FC.DEFAULT_TORQUE_CONTROLLER_PARAMS)
skill.add_termination_params(FC.DEFAULT_TERM_PARAMS)
skill.add_trajectory_params([duration] + delta_tool_base_pose.translation.tolist() + delta_tool_base_pose.quaternion.tolist())
goal = skill.create_goal()
self._send_goal(goal, cb=lambda x: skill.feedback_callback(x))
def goto_joints(self, joints, duration=3):
'''Commands Arm to the given joint configuration
Args:
joints (list): A list of 7 numbers that correspond to joint angles in radians
duration (float) : How much time this robot motion should take
Raises:
ValueError: If is_joints_reachable(joints) returns False
FrankaArmCollisionException if a collision is detected
'''
if not self.is_joints_reachable(joints):
raise ValueError('Joints not reachable!')
skill = JointPoseWithDefaultSensorSkill()
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.add_termination_params(FC.DEFAULT_TERM_PARAMS)
skill.add_trajectory_params([duration] + np.array(joints).tolist())
goal = skill.create_goal()
self._send_goal(goal, cb=lambda x: skill.feedback_callback(x))
def apply_joint_torques(self, torques, duration):
'''Commands Arm to apply given joint torques for duration seconds
Args:
torques (list): A list of 7 numbers that correspond to torques in Nm
duration (float): A float in the unit of seconds
Raises:
FrankaArmCollisionException if a collision is detected
'''
pass
def apply_effector_forces_torques(self, duration, forces=None, torques=None):
'''Applies the given end-effector forces and torques in N and Nm
Args:
duration (float): A float in the unit of seconds
forces (list): Optional (defaults to None).
A list of 3 numbers that correspond to end-effector forces in 3 directions
torques (list): Optional (defaults to None).
A list of 3 numbers that correspond to end-effector torques in 3 axes
Raises:
FrankaArmCollisionException if a collision is detected
'''
forces = [0, 0, 0] if forces is None else np.array(forces).tolist()
torques = [0, 0, 0] if torques is None else np.array(torques).tolist()
if np.linalg.norm(forces) > FC.MAX_FORCE_NORM:
raise ValueError('Force magnitude exceeds safety threshold of {}'.format(FC.MAX_FORCE_NORM))
if np.linalg.norm(torques) > FC.MAX_TORQUE_NORM:
raise ValueError('Torque magnitude exceeds safety threshold of {}'.format(FC.MAX_TORQUE_NORM))
skill = ForceTorqueSkill()
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
skill.add_termination_params([duration])
skill.add_trajectory_params([0] + forces + torques)
goal = skill.create_goal()
self._send_goal(goal, cb=lambda x: skill.feedback_callback(x))
def goto_gripper(self, width, speed=0.04, force=None):
'''Commands gripper to goto a certain width, applying up to the given (default is max) force if needed
Args:
width (float): A float in the unit of meters
speed (float): Gripper operation speed in meters per sec
force (float): Max gripper force to apply in N. Default to None, which gives acceptable force
Raises:
ValueError: If width is less than 0 or greater than TODO(jacky) the maximum gripper opening
'''
skill = GripperWithDefaultSensorSkill()
skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES)
# TODO(jacky): why is wait time needed?
if force is not None:
skill.add_trajectory_params([width, speed, force, FC.GRIPPER_WAIT_TIME]) # Gripper Width, Gripper Speed, Wait Time
else:
skill.add_trajectory_params([width, speed, FC.GRIPPER_WAIT_TIME]) # Gripper Width, Gripper Speed, Wait Time
goal = skill.create_goal()
self._send_goal(goal, cb=lambda x: skill.feedback_callback(x))
def open_gripper(self):
'''Opens gripper to maximum width
'''
self.goto_gripper(FC.GRIPPER_WIDTH_MAX)
def close_gripper(self, grasp=True):
'''Closes the gripper as much as possible
'''
self.goto_gripper(FC.GRIPPER_WIDTH_MIN, force=FC.GRIPPER_MAX_FORCE if grasp else None)
'''
Reads
'''
def get_robot_state(self):
'''
Returns:
dict of full robot state data
'''
return self._sub.get_data()
def get_pose(self):
'''
Returns:
pose (RigidTransform) of the current end-effector
'''
tool_base_pose = self._sub.get_pose()
tool_pose = tool_base_pose * self._tool_delta_pose
return tool_pose
def get_joints(self):
'''
Returns:
ndarray of shape (7,) of joint angles in radians
'''
return self._sub.get_joints()
def get_joint_torques(self):
'''
Returns:
ndarray of shape (7,) of joint torques in Nm
'''
return self._sub.get_joint_torques()
def get_joint_velocities(self):
'''
Returns:
ndarray of shape (7,) of joint velocities in rads/s
'''
return self._sub.get_joint_velocities()
def get_gripper_width(self):
'''
Returns:
float of gripper width in meters
'''
return self._sub.get_gripper_width()
def get_gripper_is_grasped(self):
'''
Returns:
True if gripper is grasping something. False otherwise
'''
return self._sub.get_gripper_is_grasped()
def get_speed(self, speed):
'''
Returns:
float of current target speed parameter
'''
pass
def get_tool_base_pose(self):
'''
Returns:
RigidTransform of current tool base pose
'''
return self._tool_delta_pose.copy()
'''
Sets
'''
def set_tool_delta_pose(self, tool_delta_pose):
'''Sets the tool pose relative to the end-effector pose
Args:
tool_delta_pose (RigidTransform)
'''
if tool_delta_pose.from_frame != 'franka_tool' or tool_delta_pose.to_frame != 'franka_tool_base':
raise ValueError('tool_delta_pose has invalid frame names! Make sure the has from_frame=franka_tool, and to_frame=franka_tool_base')
self._tool_delta_pose = tool_delta_pose.copy()
def set_speed(self, speed):
'''Sets current target speed parameter
Args:
speed (float)
'''
pass
'''
Misc
'''
def reset_joints(self):
'''Commands Arm to goto hardcoded home joint configuration
Raises:
FrankaArmCollisionException if a collision is detected
'''
self.goto_joints(FC.HOME_JOINTS, duration=5)
def is_joints_reachable(self, joints):
'''
Returns:
True if all joints within joint limits
'''
for i, val in enumerate(joints):
if val <= FC.JOINT_LIMITS_MIN[i] or val >= FC.JOINT_LIMITS_MAX[i]:
return False
return True
