def get_link_tf(self, frame_name, timeout=5.0, parent='/panda_link8'):
listener = tf.TransformListener()
err = "FrankaFramesInterface: Error while looking up transform from Flange frame to link frame %s" % frame_name
def body():
try:
listener.lookupTransform(parent, frame_name, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
err = e
return False
return True
franka_dataflow.wait_for(lambda: body(),
timeout=timeout,
raise_on_error=True,
timeout_msg=err)
t, rot = listener.lookupTransform(parent, frame_name, rospy.Time(0))
rot = np.quaternion(rot[3], rot[0], rot[1], rot[2])
rot = quaternion.as_rotation_matrix(rot)
trans_mat = np.eye(4)
trans_mat[:3, :3] = rot
trans_mat[:3, 3] = np.array(t)
return trans_mat
def __init__(self, robot_params = None):
"""
"""
if robot_params:
self._params = robot_params
else:
self._params = RobotParams()
self._ns = self._params.get_base_namespace()
self._enabled = None
state_topic = '{}/custom_franka_state_controller/robot_state'.format(self._ns)
self._state_sub = rospy.Subscriber(state_topic,
RobotState,
self._state_callback
)
franka_dataflow.wait_for(
lambda: not self._enabled is None,
timeout=5.0,
timeout_msg=("Failed to get robot state on %s" %
(state_topic,)),
)
def set_K_frame_to_link(self, frame_name, timeout = 5.0):
"""
Set new K frame to the same frame as the link frame given by 'frame_name'
Motion controllers are stopped for switching
@type frame_name: str
@param frame_name: desired tf frame name in the tf tree
@rtype: [bool, str]
@return: [success status of service request, error msg if any]
"""
trans = False
listener = tf.TransformListener()
err = "FrankaFramesInterface: Error while looking up transform from EE frame to link frame %s"%frame_name
def body():
try:
listener.lookupTransform('/panda_EE', frame_name, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
return False
return True
franka_dataflow.wait_for(lambda: body(), timeout = timeout, raise_on_error = True, timeout_msg = err)
t,rot = listener.lookupTransform('/panda_EE', frame_name, rospy.Time(0))
rot = np.quaternion(rot[3],rot[0],rot[1],rot[2])
rot = quaternion.as_rotation_matrix(rot)
trans_mat = np.eye(4)
trans_mat[:3,:3] = rot
trans_mat[:3,3] = np.array(t)
return self.set_K_frame(trans_mat)
def goto_mp(self,
movement_primitive: MovementPrimitive,
frequency=50,
duration=10.,
group_name="arm_gripper"):
curr_controller = self._ctrl_manager.set_motion_controller(
self._ctrl_manager.joint_trajectory_controller)
traj_client = JointTrajectoryActionClient(
joint_names=self.groups[group_name].refs)
traj_client.clear()
pos_traj = movement_primitive.get_full_trajectory(frequency=frequency,
duration=duration)
vel_traj = movement_primitive.get_full_trajectory_derivative(
frequency=frequency, duration=duration)
timing = []
positions = []
velocities = []
d_tot = 0
for goal, d in pos_traj:
d_tot += float(d)
timing.append(d_tot)
positions.append([goal[k] for k in self.groups[group_name].refs])
for goal, d in vel_traj:
velocities.append(
[goal[k] / duration for k in self.groups[group_name].refs])
for t, position, velocity in zip(timing, positions, velocities):
traj_client.add_point(positions=position,
time=t,
velocities=velocity)
print("COMMAND SENT")
traj_client.start() # send the trajectory action request
# traj_client.wait(timeout = timeout)
franka_dataflow.wait_for(test=lambda: traj_client.result(),
timeout=60,
timeout_msg="Something did not work",
rate=100,
raise_on_error=False)
res = traj_client.result()
if res is not None and res.error_code:
rospy.loginfo("Trajectory Server Message: {}".format(res))
rospy.sleep(0.5)
self._ctrl_manager.set_motion_controller(curr_controller)
rospy.loginfo("{}: Trajectory controlling complete".format(
self.__class__.__name__))
rospy.sleep(0.5)
def move_from_touch(self, positions, timeout=10.0, threshold=0.00085):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter to smooth the movement.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
@type threshold: float
@param threshold: position threshold in radians across each joint when
move is considered successful [0.008726646]
@param test: optional function returning True if motion must be aborted
"""
if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller:
self.switchToController(
self._ctrl_manager.joint_trajectory_controller)
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names())
traj_client.clear()
dur = []
for j in range(len(self._joint_names)):
dur.append(
max(
abs(positions[self._joint_names[j]] -
self._joint_angle[self._joint_names[j]]) /
self._joint_limits.velocity[j], min_traj_dur))
traj_client.add_point(
positions=[positions[n] for n in self._joint_names],
time=max(dur) / self._speed_ratio)
diffs = [
self.genf(j, a) for j, a in positions.items()
if j in self._joint_angle
]
fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format(
self.name.capitalize())
traj_client.start() # send the trajectory action request
franka_dataflow.wait_for(test=lambda:
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg="Unable to complete plan!",
rate=100,
raise_on_error=False)
rospy.sleep(0.5)
rospy.loginfo("ArmInterface: Trajectory controlling complete")
def _toggle_enabled(self, status):
pub = rospy.Publisher('{}/franka_control/error_recovery/goal'.format(self._ns), ErrorRecoveryActionGoal,
queue_size=10)
if self._enabled == status:
rospy.loginfo("Robot is already %s"%self.state())
franka_dataflow.wait_for(
test=lambda: self._enabled == status,
timeout=5.0,
timeout_msg=("Failed to %sable robot" %
('en' if status else 'dis',)),
body=lambda: pub.publish(ErrorRecoveryActionGoal()),
)
rospy.loginfo("Robot %s", ('Enabled' if status else 'Disabled'))
def exit_control_mode(self, timeout=5, velocity_tolerance=1e-2):
"""
Clean exit from advanced control modes (joint torque or velocity).
Resets control to joint position mode with current positions until
further advanced control commands are sent to the robot.
.. note:: In normal cases, this method is not required as the
interface automatically switches to position control mode if
advanced control commands (velocity/torque) are not sent at
regular intervals. Therefore it is enough to stop sending the
commands to disable advanced control modes.
.. note:: In sim, this method does nothing.
:type timeout: float
:param timeout: seconds to wait for robot to stop moving before giving up [default: 5]
:type velocity_tolerance: float
:param velocity_tolerance: tolerance
"""
if self._params._in_sim: return
self.set_command_timeout(0.05)
rospy.sleep(0.5)
def check_stop():
return np.allclose(np.asarray(list(self._joint_velocity.values())),
0.,
atol=velocity_tolerance)
rospy.loginfo(
"{}: Waiting for robot to stop moving to exit control mode...".
format(self.__class__.__name__))
franka_dataflow.wait_for(
test=lambda: check_stop(),
timeout=timeout,
timeout_msg=
"{}: FAILED to exit control mode! The robot may be still moving. Controllers might not switch correctly"
.format(self.__class__.__name__),
rate=20,
raise_on_error=False)
rospy.loginfo(
"{}: Done. Setting position control target to current position.".
format(self.__class__.__name__))
self.set_joint_positions(self.joint_angles())
def get_link_tf(self, frame_name, timeout=5.0, parent='panda_NE'):
"""
Get :math:`4\times 4` transformation matrix of a frame with respect to another.
:return: :math:`4\times 4` transformation matrix
:rtype: np.ndarray
:param frame_name: Name of the child frame from the TF tree
:type frame_name: str
:param parent: Name of parent frame (default: 'panda_NE', the default nominal end-effector frame set in Desk)
:type parent: str
"""
tfBuffer = tf.Buffer()
listener = tf.TransformListener(tfBuffer)
err = "FrankaFramesInterface: Error while looking up transform from Flange frame to link frame %s" % frame_name
def body():
try:
tfBuffer.lookup_transform(parent, frame_name, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return False
return True
franka_dataflow.wait_for(
lambda: body(), timeout=timeout, raise_on_error=True, timeout_msg=err)
trans_stamped = tfBuffer.lookup_transform(
parent, frame_name, rospy.Time(0))
t = [trans_stamped.transform.translation.x,
trans_stamped.transform.translation.y, trans_stamped.transform.translation.z]
q = trans_stamped.transform.rotation
rot = np.quaternion(q.w, q.x, q.y, q.z)
rot = quaternion.as_rotation_matrix(rot)
trans_mat = np.eye(4)
trans_mat[:3, :3] = rot
trans_mat[:3, 3] = np.array(t)
return trans_mat
def get_link_tf(self, frame_name, timeout=5.0, parent='/panda_link8'):
"""
Get :math:`4\times 4` transformation matrix of a frame with respect to another.
:return: :math:`4\times 4` transformation matrix
:rtype: np.ndarray
:param frame_name: Name of the child frame from the TF tree
:type frame_name: str
:param parent: Name of parent frame (default: '/panda_link8')
:type parent: str
"""
listener = tf.TransformListener()
err = "FrankaFramesInterface: Error while looking up transform from Flange frame to link frame %s" % frame_name
def body():
try:
listener.lookupTransform(parent, frame_name, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return False
return True
franka_dataflow.wait_for(lambda: body(),
timeout=timeout,
raise_on_error=True,
timeout_msg=err)
t, rot = listener.lookupTransform(parent, frame_name, rospy.Time(0))
rot = np.quaternion(rot[3], rot[0], rot[1], rot[2])
rot = quaternion.as_rotation_matrix(rot)
trans_mat = np.eye(4)
trans_mat[:3, :3] = rot
trans_mat[:3, 3] = np.array(t)
return trans_mat
def __init__(self, synchronous_pub=False):
"""
"""
self.hand = franka_interface.GripperInterface()
self._params = RobotParams()
self._ns = self._params.get_base_namespace()
self._joint_limits = self._params.get_joint_limits()
joint_names = self._joint_limits.joint_names
if not joint_names:
rospy.logerr("Cannot detect joint names for arm on this "
"robot. Exiting Arm.init().")
return
self._joint_names = joint_names
self.name = self._params.get_robot_name()
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
self._stiffness_frame_effort = dict()
self._errors = dict()
self._collision_state = False
self._tip_states = None
self._jacobian = None
self._cartesian_contact = None
self._robot_mode = False
self._command_msg = JointCommand()
# neutral pose joint positions
self._neutral_pose_joints = self._params.get_neutral_pose()
self._frames_interface = FrankaFramesInterface()
try:
self._collision_behaviour_interface = CollisionBehaviourInterface()
except rospy.ROSException:
rospy.loginfo(
"Collision Service Not found. It will not be possible to change collision behaviour of robot!"
)
self._collision_behaviour_interface = None
self._ctrl_manager = FrankaControllerManagerInterface(
ns=self._ns, sim=self._params._in_sim)
self._speed_ratio = 0.15
queue_size = None if synchronous_pub else 1
with warnings.catch_warnings():
warnings.simplefilter("ignore")
self._joint_command_publisher = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=queue_size)
self._pub_joint_cmd_timeout = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_command_timeout',
Float64,
latch=True,
queue_size=10)
self._robot_state_subscriber = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/robot_state',
RobotState,
self._on_robot_state,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = self._ns + '/custom_franka_state_controller/joint_states'
self._joint_state_sub = rospy.Subscriber(joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
self._cartesian_state_sub = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/tip_state',
EndPointState,
self._on_endpoint_state,
queue_size=1,
tcp_nodelay=True)
# Cartesian Impedance Controller Publishers
self._cartesian_impedance_pose_publisher = rospy.Publisher(
"equilibrium_pose", PoseStamped, queue_size=10)
self._cartesian_stiffness_publisher = rospy.Publisher(
"impedance_stiffness", CartImpedanceStiffness, queue_size=10)
# Force Control Publisher
self._force_controller_publisher = rospy.Publisher("wrench_target",
Wrench,
queue_size=10)
# Torque Control Publisher
self._torque_controller_publisher = rospy.Publisher("torque_target",
TorqueCmd,
queue_size=20)
# Joint Impedance Controller Publishers
self._joint_impedance_publisher = rospy.Publisher(
"joint_impedance_position_velocity", JICmd, queue_size=20)
self._joint_stiffness_publisher = rospy.Publisher(
"joint_impedance_stiffness",
JointImpedanceStiffness,
queue_size=10)
rospy.on_shutdown(self._clean_shutdown)
err_msg = ("%s arm init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
franka_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current tip_state "
"from %s") % (self.name.capitalize(),
self._ns + 'tip_state')
franka_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current robot_state "
"from %s") % (self.name.capitalize(),
self._ns + 'robot_state')
franka_dataflow.wait_for(lambda: self._jacobian is not None,
timeout_msg=err_msg,
timeout=5.0)
self.set_joint_position_speed(self._speed_ratio)
def move_to_joint_positions(self,
positions,
timeout=10.0,
threshold=0.00085,
test=None,
use_moveit=True):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter using JointTrajectoryService
to smooth the movement or optionally uses MoveIt! to plan and
execute a trajectory.
:type positions: dict({str:float})
:param positions: joint_name:angle command
:type timeout: float
:param timeout: seconds to wait for move to finish [15]
:type threshold: float
:param threshold: position threshold in radians across each joint when
move is considered successful [0.00085]
:param test: optional function returning True if motion must be aborted
:type use_moveit: bool
:param use_moveit: if set to True, and movegroup interface is available,
move to the joint positions using moveit planner.
"""
curr_controller = self._ctrl_manager.set_motion_controller(
self._ctrl_manager.joint_trajectory_controller)
if use_moveit and self._movegroup_interface:
self._movegroup_interface.go_to_joint_positions(
[positions[n] for n in self._joint_names], tolerance=threshold)
else:
if use_moveit:
rospy.logwarn(
"{}: MoveGroupInterface was not found! Using JointTrajectoryActionClient instead."
.format(self.__class__.__name__))
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names())
traj_client.clear()
dur = []
for j in range(len(self._joint_names)):
dur.append(
max(
abs(positions[self._joint_names[j]] -
self._joint_angle[self._joint_names[j]]) /
self._joint_limits.velocity[j], min_traj_dur))
traj_client.add_point(
positions=[self._joint_angle[n] for n in self._joint_names],
time=0.0001)
traj_client.add_point(
positions=[positions[n] for n in self._joint_names],
time=max(dur) / self._speed_ratio)
def genf(joint, angle):
def joint_diff():
return abs(angle - self._joint_angle[joint])
return joint_diff
diffs = [
genf(j, a) for j, a in positions.items()
if j in self._joint_angle
]
fail_msg = "{}: {} limb failed to reach commanded joint positions.".format(
self.__class__.__name__, self.name.capitalize())
def test_collision():
if self.has_collided():
rospy.logerr(' '.join(["Collision detected.", fail_msg]))
return True
return False
traj_client.start() # send the trajectory action request
# traj_client.wait(timeout = timeout)
franka_dataflow.wait_for(
test=lambda: test_collision() or traj_client.result(
) is not None or (callable(test) and test() == True) or
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg=fail_msg,
rate=100,
raise_on_error=False)
res = traj_client.result()
if res is not None and res.error_code:
rospy.loginfo("Trajectory Server Message: {}".format(res))
rospy.sleep(0.5)
self._ctrl_manager.set_motion_controller(curr_controller)
rospy.loginfo("{}: Trajectory controlling complete".format(
self.__class__.__name__))
def __init__(self, synchronous_pub=False):
self._params = RobotParams()
self._ns = self._params.get_base_namespace()
self._joint_limits = self._params.get_joint_limits()
joint_names = self._joint_limits.joint_names
if not joint_names:
rospy.logerr("{}: Cannot detect joint names for arm on this "
"robot. Exiting Arm.init().".format(
self.__class__.__name__))
return
self._joint_names = joint_names
self.name = self._params.get_robot_name()
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
self._stiffness_frame_effort = dict()
self._errors = dict()
self._collision_state = False
self._tip_states = None
self._jacobian = None
self._cartesian_contact = None
self._robot_mode = False
self._command_msg = JointCommand()
# neutral pose joint positions
self._neutral_pose_joints = self._params.get_neutral_pose()
self._frames_interface = FrankaFramesInterface()
try:
self._collision_behaviour_interface = CollisionBehaviourInterface()
except rospy.ROSException:
rospy.loginfo(
"{}: Collision Service Not found. It will not be possible to change collision behaviour of robot!"
.format(self.__class__.__name__))
self._collision_behaviour_interface = None
self._ctrl_manager = FrankaControllerManagerInterface(
ns=self._ns, sim=self._params._in_sim)
self._speed_ratio = 0.15
self._F_T_NE = np.eye(1).flatten().tolist()
self._NE_T_EE = np.eye(1).flatten().tolist()
queue_size = None if synchronous_pub else 1
with warnings.catch_warnings():
warnings.simplefilter("ignore")
self._joint_command_publisher = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=queue_size)
self._pub_joint_cmd_timeout = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_command_timeout',
Float64,
latch=True,
queue_size=10)
self._robot_state_subscriber = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/robot_state',
RobotState,
self._on_robot_state,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = self._ns + '/custom_franka_state_controller/joint_states'
self._joint_state_sub = rospy.Subscriber(joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
self._cartesian_state_sub = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/tip_state',
EndPointState,
self._on_endpoint_state,
queue_size=1,
tcp_nodelay=True)
rospy.on_shutdown(self._clean_shutdown)
err_msg = ("%s arm init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
franka_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current tip_state "
"from %s") % (self.name.capitalize(),
self._ns + 'tip_state')
franka_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current robot_state "
"from %s") % (self.name.capitalize(),
self._ns + 'robot_state')
franka_dataflow.wait_for(lambda: self._jacobian is not None,
timeout_msg=err_msg,
timeout=5.0)
# start moveit server with panda_link8 (flange) as the end-effector, unless it is in simulation. However, using move_to_cartesian_pose() method compensates for this and moves the robot's defined end-effector frame (EE frame; see set_EE_frame() and set_EE_at_frame()) to the target pose.
try:
self._movegroup_interface = PandaMoveGroupInterface(
use_panda_hand_link=True if self._params._in_sim else False)
except:
rospy.loginfo(
"{}: MoveGroup was not found! This is okay if moveit service is not required!"
.format(self.__class__.__name__))
self._movegroup_interface = None
self.set_joint_position_speed(self._speed_ratio)
def __init__(self,
gripper_joint_names=('panda_finger_joint1',
'panda_finger_joint2'),
calibrate=False,
**kwargs):
"""
Constructor.
"""
self.name = '/franka_gripper'
ns = self.name + '/'
self._joint_positions = dict()
self._joint_names = gripper_joint_names
self._joint_velocity = dict()
self._joint_effort = dict()
self._joint_states_state_sub = rospy.Subscriber(
ns + 'joint_states',
JointState,
self._joint_states_callback,
queue_size=1,
tcp_nodelay=True)
self._exists = False
# ----- Initial test to see if gripper is loaded
try:
rospy.get_param("/franka_gripper/robot_ip")
except KeyError:
rospy.loginfo("FrankaGripper: could not detect gripper.")
return
except (socket.error, socket.gaierror):
print("Failed to connect to the ROS parameter server!\n"
"Please check to make sure your ROS networking is "
"properly configured:\n")
sys.exit()
# ----- Wait for the gripper device status to be true
if not franka_dataflow.wait_for(
lambda: len(list(self._joint_positions.keys())) > 0,
timeout=2.0,
timeout_msg=
("FrankaGripper: Failed to get gripper joint positions. Assuming no gripper attached to robot."
),
raise_on_error=False):
return
self._exists = True
self._gripper_speed = 0.05
self._homing_action_client = actionlib.SimpleActionClient(
"{}homing".format(ns), HomingAction)
self._grasp_action_client = actionlib.SimpleActionClient(
"{}grasp".format(ns), GraspAction)
self._move_action_client = actionlib.SimpleActionClient(
"{}move".format(ns), MoveAction)
self._stop_action_client = actionlib.SimpleActionClient(
"{}stop".format(ns), StopAction)
rospy.loginfo(
"GripperInterface: Waiting for gripper action servers... ")
self._homing_action_client.wait_for_server()
self._grasp_action_client.wait_for_server()
self._move_action_client.wait_for_server()
self._stop_action_client.wait_for_server()
rospy.loginfo("GripperInterface: Gripper action servers found! ")
self.MIN_FORCE = 0.01
self.MAX_FORCE = 50 # documentation says upto 70N is possible as continuous force (max upto 140N)
self.MIN_WIDTH = 0.0001
self.MAX_WIDTH = 0.2
if calibrate:
self.calibrate()
def execute_position_path(self,
position_path,
timeout=15.0,
threshold=0.00085,
test=None):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter to smooth the movement.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
@type threshold: float
@param threshold: position threshold in radians across each joint when
move is considered successful [0.008726646]
@param test: optional function returning True if motion must be aborted
"""
current_q = self.joint_angles()
diff_from_start = sum(
[abs(a - current_q[j]) for j, a in position_path[0].items()])
if diff_from_start > 0.1:
raise IOError("Robot not at start of trajectory")
if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller:
self.switchToController(
self._ctrl_manager.joint_trajectory_controller)
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names())
traj_client.clear()
time_so_far = 0
# Start at the second waypoint because robot is already at first waypoint
for i in xrange(1, len(position_path)):
q = position_path[i]
dur = []
for j in range(len(self._joint_names)):
dur.append(
max(
abs(q[self._joint_names[j]] -
self._joint_angle[self._joint_names[j]]) /
self._joint_limits.velocity[j], min_traj_dur))
time_so_far += max(dur) / self._speed_ratio
traj_client.add_point(
positions=[q[n] for n in self._joint_names],
time=time_so_far,
velocities=[0.005 for n in self._joint_names])
diffs = [
self.genf(j, a) for j, a in (position_path[-1]).items()
if j in self._joint_angle
] # Measures diff to last waypoint
fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format(
self.name.capitalize())
def test_collision():
if self.has_collided():
rospy.logerr(' '.join(["Collision detected.", fail_msg]))
return True
return False
traj_client.start() # send the trajectory action request
franka_dataflow.wait_for(
test=lambda: test_collision() or \
(callable(test) and test() == True) or \
(all(diff() < threshold for diff in diffs)),
#timeout=timeout,
timeout=max(time_so_far, timeout), #XXX
timeout_msg=fail_msg,
rate=100,
raise_on_error=False
)
rospy.sleep(0.5)
rospy.loginfo("ArmInterface: Trajectory controlling complete")
def move_to_touch(self, positions, timeout=10.0, threshold=0.00085):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter to smooth the movement.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
@type threshold: float
@param threshold: position threshold in radians across each joint when
move is considered successful [0.008726646]
@param test: optional function returning True if motion must be aborted
"""
if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller:
self.switchToController(
self._ctrl_manager.joint_trajectory_controller)
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names())
traj_client.clear()
speed_ratio = 0.05 # Move slower when approaching contact
dur = []
for j in range(len(self._joint_names)):
dur.append(
max(
abs(positions[self._joint_names[j]] -
self._joint_angle[self._joint_names[j]]) /
self._joint_limits.velocity[j], min_traj_dur))
traj_client.add_point(
positions=[positions[n] for n in self._joint_names],
time=max(dur) / speed_ratio,
velocities=[0.002 for n in self._joint_names])
diffs = [
self.genf(j, a) for j, a in positions.items()
if j in self._joint_angle
]
fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format(
self.name.capitalize())
traj_client.start() # send the trajectory action request
franka_dataflow.wait_for(
test=lambda: self.has_collided() or \
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg="Move to touch complete.",
rate=100,
raise_on_error=False
)
rospy.sleep(0.5)
if not self.has_collided():
rospy.logerr('Move To Touch did not end in making contact')
else:
rospy.loginfo('Collision detected!')
# The collision, though desirable, triggers a cartesian reflex error. We need to reset that error
if self._robot_mode == 4:
self.resetErrors()
rospy.loginfo("ArmInterface: Trajectory controlling complete")
def __init__(
self,
gripper_joint_names=['panda_finger_joint1', 'panda_finger_joint2'],
ns='franka_ros_interface',
calibrate=False):
"""
Constructor.
@param gripper_joint_names : Names of the finger joints
@param ns : base namespace of interface ('frank_ros_interface'/'panda_simulator')
@param calibrate : Attempts to calibrate the gripper when initializing class (defaults True)
@type calibrate : bool
@type gripper_joint_names : [str]
@type ns : str
"""
self.name = ns + '/franka_gripper'
ns = self.name + '/'
self._joint_positions = dict()
self._joint_names = gripper_joint_names
self._joint_velocity = dict()
self._joint_effort = dict()
self._joint_states_state_sub = rospy.Subscriber(
ns + 'joint_states',
JointState,
self._joint_states_callback,
queue_size=1,
tcp_nodelay=True)
# ----- Wait for the gripper device status to be true
if not franka_dataflow.wait_for(
lambda: len(self._joint_positions.keys()) > 0,
timeout=2.0,
timeout_msg=
("FrankaGripper: Failed to get gripper. No gripper attached on the robot."
),
raise_on_error=False):
self._exists = False
return
self._exists = True
if not self._exists:
return
self._gripper_speed = 0.05
self._homing_action_client = actionlib.SimpleActionClient(
"{}homing".format(ns), HomingAction)
self._grasp_action_client = actionlib.SimpleActionClient(
"{}grasp".format(ns), GraspAction)
self._move_action_client = actionlib.SimpleActionClient(
"{}move".format(ns), MoveAction)
self._stop_action_client = actionlib.SimpleActionClient(
"{}stop".format(ns), StopAction)
rospy.loginfo(
"GripperInterface: Waiting for gripper action servers... ")
self._homing_action_client.wait_for_server()
self._grasp_action_client.wait_for_server()
self._move_action_client.wait_for_server()
self._stop_action_client.wait_for_server()
rospy.loginfo("GripperInterface: Gripper action servers found! ")
self.MIN_FORCE = 0.01
self.MAX_FORCE = 50 # documentation says upto 70N is possible as continuous force (max upto 140N)
self.MIN_WIDTH = 0.0001
self.MAX_WIDTH = 0.2
if calibrate:
self.calibrate()
def move_to_joint_positions(self,
positions,
timeout=10.0,
threshold=0.00085,
test=None):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter to smooth the movement.
@type positions: dict({str:float})
@param positions: joint_name:angle command
@type timeout: float
@param timeout: seconds to wait for move to finish [15]
@type threshold: float
@param threshold: position threshold in radians across each joint when
move is considered successful [0.008726646]
@param test: optional function returning True if motion must be aborted
"""
if self._params._in_sim:
rospy.warn(
"ArmInterface: move_to_joint_positions not implemented for simulation. Use set_joint_positions instead."
)
return
switch_ctrl = True if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller else False
if switch_ctrl:
active_controllers = self._ctrl_manager.list_active_controllers(
only_motion_controllers=True)
for ctrlr in active_controllers:
self._ctrl_manager.stop_controller(ctrlr.name)
rospy.loginfo(
"ArmInterface: Stopping %s for trajectory controlling" %
ctrlr.name)
rospy.sleep(0.5)
if not self._ctrl_manager.is_loaded(
self._ctrl_manager.joint_trajectory_controller):
self._ctrl_manager.load_controller(
self._ctrl_manager.joint_trajectory_controller)
# if self._ctrl_manager.joint_trajectory_controller not in self._ctrl_manager.list_active_controller_names():
self._ctrl_manager.start_controller(
self._ctrl_manager.joint_trajectory_controller)
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names(), ns=self._ns)
traj_client.clear()
dur = []
for j in range(len(self._joint_names)):
dur.append(
max(
abs(positions[self._joint_names[j]] -
self._joint_angle[self._joint_names[j]]) /
self._joint_limits.velocity[j], min_traj_dur))
traj_client.add_point(
positions=[positions[n] for n in self._joint_names],
time=max(dur) / self._speed_ratio)
def genf(joint, angle):
def joint_diff():
return abs(angle - self._joint_angle[joint])
return joint_diff
diffs = [
genf(j, a) for j, a in positions.items() if j in self._joint_angle
]
fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format(
self.name.capitalize())
def test_collision():
if self.has_collided():
rospy.logerr(' '.join(["Collision detected.", fail_msg]))
return True
return False
traj_client.start() # send the trajectory action request
# traj_client.wait(timeout = timeout)
franka_dataflow.wait_for(
test=lambda: test_collision() or \
(callable(test) and test() == True) or \
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg=fail_msg,
rate=100,
raise_on_error=False
)
rospy.sleep(0.5)
if switch_ctrl:
self._ctrl_manager.stop_controller(
self._ctrl_manager.joint_trajectory_controller)
for ctrlr in active_controllers:
self._ctrl_manager.start_controller(ctrlr.name)
rospy.loginfo("ArmInterface: Restaring %s" % ctrlr.name)
rospy.sleep(0.5)
rospy.loginfo("ArmInterface: Trajectory controlling complete")
def _on_trajectory_action(self, goal):
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
# Load parameters for trajectory
self._get_trajectory_parameters(joint_names, goal)
# Create a new discretized joint trajectory
num_points = len(trajectory_points)
if num_points == 0:
rospy.logerr("JointTrajectoryActionServer: Empty Trajectory")
self._server.set_aborted()
return
rospy.loginfo("JointTrajectoryActionServer: Executing requested joint trajectory")
rospy.logdebug("Trajectory Points: {0}".format(trajectory_points))
for jnt_name, jnt_value in self._get_current_error(
joint_names, trajectory_points[0].positions):
if abs(self._path_thresh[jnt_name]) < abs(jnt_value):
rospy.logerr(("JointTrajectoryActionServer: Initial Trajectory point violates "
"threshold on joint {0} with delta {1} radians. "
"Aborting trajectory execution.").format(jnt_name, jnt_value))
self._server.set_aborted()
return
control_rate = rospy.Rate(self._control_rate)
dimensions_dict = self._determine_dimensions(trajectory_points)
# Force Velocites/Accelerations to zero at the final timestep
# if they exist in the trajectory
# Remove this behavior if you are stringing together trajectories,
# and want continuous, non-zero velocities/accelerations between
# trajectories
if dimensions_dict['velocities']:
trajectory_points[-1].velocities = [0.0] * len(joint_names)
if dimensions_dict['accelerations']:
trajectory_points[-1].accelerations = [0.0] * len(joint_names)
# Compute Full Bezier Curve Coefficients for all 7 joints
pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
try:
if self._interpolation == 'minjerk':
# Compute Full MinJerk Curve Coefficients for all 7 joints
point_duration = [pnt_times[i+1] - pnt_times[i] for i in range(len(pnt_times)-1)]
m_matrix = self._compute_minjerk_coeff(joint_names,
trajectory_points,
point_duration,
dimensions_dict)
else:
# Compute Full Bezier Curve Coefficients for all 7 joints
b_matrix = self._compute_bezier_coeff(joint_names,
trajectory_points,
dimensions_dict)
# for i in range(-5,0):
# if dimensions_dict['velocities']:
# trajectory_points[-1].velocities = [0.00001] * len(joint_names)
# trajectory_points[-2].velocities = [0.01] * len(joint_names)
# if dimensions_dict['accelerations']:
# trajectory_points[-1].accelerations = [0.00001] * len(joint_names)
# trajectory_points[-2].accelerations = [0.01] * len(joint_names)
except Exception as ex:
rospy.logerr(("JointTrajectoryActionServer: Failed to compute a Bezier trajectory for panda"
" arm with error \"{}: {}\"").format(
type(ex).__name__, ex))
self._server.set_aborted()
return
# Wait for the specified execution time, if not provided use now
start_time = goal.trajectory.header.stamp.to_sec()
if start_time == 0.0:
start_time = rospy.get_time()
franka_dataflow.wait_for(
lambda: rospy.get_time() >= start_time,
timeout=float('inf')
)
# Loop until end of trajectory time. Provide a single time step
# of the control rate past the end to ensure we get to the end.
# Keep track of current indices for spline segment generation
now_from_start = rospy.get_time() - start_time
end_time = trajectory_points[-1].time_from_start.to_sec()
while (now_from_start < end_time and not rospy.is_shutdown() and
self.robot_is_enabled()):
now = rospy.get_time()
now_from_start = now - start_time
idx = bisect.bisect(pnt_times, now_from_start)
#Calculate percentage of time passed in this interval
if idx >= num_points:
cmd_time = now_from_start - pnt_times[-1]
t = 1.0
elif idx >= 0:
cmd_time = (now_from_start - pnt_times[idx-1])
t = cmd_time / (pnt_times[idx] - pnt_times[idx-1])
else:
cmd_time = 0
t = 0
if self._interpolation == 'minjerk':
point = self._get_minjerk_point(m_matrix, idx,
t, cmd_time,
dimensions_dict)
else:
point = self._get_bezier_point(b_matrix, idx,
t, cmd_time,
dimensions_dict)
# Command Joint Position, Velocity, Acceleration
command_executed = self._command_joints(joint_names, point, start_time, dimensions_dict)
self._update_feedback(point, joint_names, now_from_start)
if not command_executed:
return
# Sleep to make sure the publish is at a consistent time
control_rate.sleep()
# Keep trying to meet goal until goal_time constraint expired
last = trajectory_points[-1]
last_time = trajectory_points[-1].time_from_start.to_sec()
end_angles = dict(zip(joint_names, last.positions))
def check_goal_state():
for error in self._get_current_error(joint_names, last.positions):
if (self._goal_error[error[0]] > 0
and self._goal_error[error[0]] < math.fabs(error[1])):
return error[0]
if (self._stopped_velocity > 0.0 and
max([abs(cur_vel) for cur_vel in self._get_current_velocities(joint_names)]) >
self._stopped_velocity):
return False
else:
return True
while (now_from_start < (last_time + self._goal_time)
and not rospy.is_shutdown() and self.robot_is_enabled()):
if not self._command_joints(joint_names, last, start_time, dimensions_dict):
return
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names,
now_from_start)
control_rate.sleep()
now_from_start = rospy.get_time() - start_time
self._update_feedback(deepcopy(last), joint_names,
now_from_start)
# Verify goal constraint
result = check_goal_state()
if result is True:
msg = "JointTrajectoryActionServer: Joint Trajectory Action Succeeded for panda arm"
rospy.loginfo(msg)
self._result.error_code = self._result.SUCCESSFUL
self._result.error_string = msg
self._server.set_succeeded(self._result)
elif result is False:
msg = "JointTrajectoryActionServer: Exceeded Max Goal Velocity Threshold for panda arm"
rospy.logerr(msg)
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._result.error_string = msg
self._server.set_aborted(self._result)
else:
msg = "JointTrajectoryActionServer: Exceeded Goal Threshold Error %s for panda arm"%(result)
rospy.logerr(msg)
self._result.error_code = self._result.GOAL_TOLERANCE_VIOLATED
self._result.error_string = msg
self._server.set_aborted(self._result)
def move_to_joint_positions(self,
positions,
timeout=10.0,
threshold=0.00085,
test=None):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter using JointTrajectoryService
to smooth the movement or optionally uses MoveIt! to plan and
execute a trajectory.
:type positions: dict({str:float})
:param positions: joint_name:angle command
:type timeout: float
:param timeout: seconds to wait for move to finish [15]
:type threshold: float
:param threshold: position threshold in radians across each joint when
move is considered successful [0.00085]
:param test: optional function returning True if motion must be aborted
"""
if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller:
self.switchToController(
self._ctrl_manager.joint_trajectory_controller)
min_traj_dur = 0.5
traj_client = JointTrajectoryActionClient(
joint_names=self.joint_names())
traj_client.clear()
dur = []
for j in range(len(self._joint_names)):
dur.append(
max(
abs(positions[self._joint_names[j]] -
self._joint_angle[self._joint_names[j]]) /
self._joint_limits.velocity[j], min_traj_dur))
traj_client.add_point(
positions=[positions[n] for n in self._joint_names],
time=max(dur) / self._speed_ratio)
diffs = [
self.genf(j, a) for j, a in positions.items()
if j in self._joint_angle
]
traj_client.start() # send the trajectory action request
fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format(
self.name.capitalize())
def test_collision():
if self.has_collided():
rospy.logerr(' '.join(["Collision detected.", fail_msg]))
return True
return False
franka_dataflow.wait_for(
test=lambda: test_collision() or \
(callable(test) and test() == True) or \
(all(diff() < threshold for diff in diffs)),
timeout=timeout,
timeout_msg=fail_msg,
rate=100,
raise_on_error=False
)
res = traj_client.result()
if res is not None and res.error_code:
rospy.loginfo("Trajectory Server Message: {}".format(res))
rospy.sleep(0.5)
rospy.loginfo("ArmInterface: Trajectory controlling complete")
def go_to(self,
trajectory: NamedTrajectoryBase,
group_name="arm_gripper",
frequency=50):
"""
(Blocking) Commands the limb to the provided positions.
Waits until the reported joint state matches that specified.
This function uses a low-pass filter using JointTrajectoryService
to smooth the movement or optionally uses MoveIt! to plan and
execute a trajectory.
:type positions: dict({str:float})
:param positions: joint_name:angle command
:type timeout: float
:param timeout: seconds to wait for move to finish [15]
:type threshold: float
:param threshold: position threshold in radians across each joint when
move is considered successful [0.00085]
:param test: optional function returning True if motion must be aborted
:type use_moveit: bool
:param use_moveit: if set to True, and movegroup interface is available,
move to the joint positions using moveit planner.
"""
curr_controller = self._ctrl_manager.set_motion_controller(
self._ctrl_manager.joint_trajectory_controller)
# traj_client = JointTrajectoryActionClient(
# joint_names=self.joint_names())
traj_client = JointTrajectoryActionClient(
joint_names=self.groups[group_name].refs)
traj_client.clear()
if len(trajectory.duration) > 1:
self._get_cubic_spline(trajectory, group_name)
for i, (position, velocity, d) \
in enumerate(zip(*self._get_cubic_spline(trajectory, group_name, frequency=frequency))):
traj_client.add_point(positions=position.tolist(),
time=float(d),
velocities=velocity.tolist())
else:
for goal, d in trajectory:
position = [
float(goal[k]) for k in self.groups[group_name].refs
]
velocity = [0.] * len(position)
print("position", position)
print("velocity", velocity)
print("d", float(d))
traj_client.add_point(positions=position,
time=float(d),
velocities=velocity)
traj_client.start() # send the trajectory action request
# traj_client.wait(timeout = timeout)
franka_dataflow.wait_for(test=lambda: traj_client.result(),
timeout=260,
timeout_msg="Something did not work",
rate=100,
raise_on_error=False)
res = traj_client.result()
if res is not None and res.error_code:
rospy.loginfo("Trajectory Server Message: {}".format(res))
rospy.sleep(0.5)
self._ctrl_manager.set_motion_controller(curr_controller)
rospy.loginfo("{}: Trajectory controlling complete".format(
self.__class__.__name__))
rospy.sleep(0.5)
def __init__(self, synchronous_pub=False):
"""
Constructor.
@type synchronous_pub: bool
@param synchronous_pub: designates the JointCommand Publisher
as Synchronous if True and Asynchronous if False.
Synchronous Publishing means that all joint_commands publishing to
the robot's joints will block until the message has been serialized
into a buffer and that buffer has been written to the transport
of every current Subscriber. This yields predicable and consistent
timing of messages being delivered from this Publisher. However,
when using this mode, it is possible for a blocking Subscriber to
prevent the joint_command functions from exiting. Unless you need exact
JointCommand timing, default to Asynchronous Publishing (False).
"""
self._params = RobotParams()
self._ns = self._params.get_base_namespace()
self._joint_limits = self._params.get_joint_limits()
joint_names = self._joint_limits.joint_names
if not joint_names:
rospy.logerr("Cannot detect joint names for arm on this "
"robot. Exiting Arm.init().")
return
self._joint_names = joint_names
self.name = self._params.get_robot_name()
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
self._stiffness_frame_effort = dict()
self._errors = dict()
self._collision_state = False
self._tip_states = None
self._jacobian = None
self._cartesian_contact = None
self._robot_mode = False
self._command_msg = JointCommand()
# neutral pose joint positions
self._neutral_pose_joints = self._params.get_neutral_pose()
self._frames_interface = FrankaFramesInterface()
self._ctrl_manager = FrankaControllerManagerInterface(
ns=self._ns, sim=True if self._params._in_sim else False)
self._speed_ratio = 0.15
queue_size = None if synchronous_pub else 1
with warnings.catch_warnings():
warnings.simplefilter("ignore")
self._joint_command_publisher = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=queue_size)
self._pub_joint_cmd_timeout = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_command_timeout',
Float64,
latch=True,
queue_size=10)
self._robot_state_subscriber = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/robot_state',
RobotState,
self._on_robot_state,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = self._ns + '/custom_franka_state_controller/joint_states' if not self._params._in_sim else self._ns + '/joint_states'
self._joint_state_sub = rospy.Subscriber(joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
self._cartesian_state_sub = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/tip_state',
EndPointState,
self._on_endpoint_state,
queue_size=1,
tcp_nodelay=True)
rospy.on_shutdown(self._clean_shutdown)
err_msg = ("%s arm init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
franka_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current tip_state "
"from %s") % (self.name.capitalize(),
self._ns + 'tip_state')
franka_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current robot_state "
"from %s") % (self.name.capitalize(),
self._ns + 'robot_state')
franka_dataflow.wait_for(lambda: self._jacobian is not None,
timeout_msg=err_msg,
timeout=5.0)