def move_to_joint_positions(self,
positions,
speed=None,
acceleration=None):
speed = speed if speed is not None else self.speed
acceleration = acceleration if acceleration is not None else self.acceleration
traj = MotionTrajectory(limb=self.limb)
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=speed,
max_joint_accel=acceleration)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=self.limb)
if isinstance(positions, dict):
joint_angles = positions.values()
waypoint.set_joint_angles(joint_angles=joint_angles)
traj.set_joint_names(positions.keys())
traj.append_waypoint(waypoint.to_msg())
else:
rospy.logerr("Incorrect inputs to move_to_joint_positions")
return
result = traj.send_trajectory(timeout=30)
self.last_activity = rospy.Time.now()
if result is None:
rospy.logerr("Trajectory failed to send")
elif not result.result:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
def execute_trajectory(self,
trajectory,
joint_names,
speed=None,
acceleration=None):
"""
trajectory is a list of points: approach (joint), init (cart), drawing (cart), retreat (cart)
"""
speed = speed if speed is not None else self.speed
acceleration = acceleration if acceleration is not None else self.acceleration
if isinstance(trajectory, dict) and "approach" in trajectory:
for mtype in ["approach", "init", "drawing", "retreat"]:
points = trajectory[mtype]
if points["type"] == "joint":
self._seed = points["joints"]
self.move_to_joint_positions(
dict(zip(joint_names, points["joints"])))
elif points["type"] == "cart":
wpt_opts = MotionWaypointOptions(
max_joint_speed_ratio=speed,
max_joint_accel=acceleration)
traj = MotionTrajectory(limb=self.limb)
waypoint = MotionWaypoint(options=wpt_opts, limb=self.limb)
t_opt = TrajectoryOptions(
interpolation_type=TrajectoryOptions.CARTESIAN)
jv = deepcopy(points["joints"])
jv.reverse()
waypoint.set_joint_angles(jv)
waypoint.set_cartesian_pose(
list_to_pose_stamped(points["pose"], frame_id="base"))
traj.append_waypoint(waypoint)
jn = [str(j) for j in joint_names]
jn.reverse()
traj.set_joint_names(jn)
traj.set_trajectory_options(t_opt)
result = traj.send_trajectory(timeout=10)
self.last_activity = rospy.Time.now()
if result is None:
rospy.logerr("Trajectory failed to send")
elif not result.result:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
else:
rospy.logwarn("Unknown type %", mtype)
else:
rospy.logerr("Incorrect inputs to execute_trajectory")
return
class Robot(object):
def __init__(self, joint_names):
self._motion = MotionTrajectory()
self.joint_names = joint_names
def execute(self, trajectory):
self._motion.clear_waypoints()
self._motion.set_joint_names(self.joint_names)
for point in trajectory:
waypoint = MotionWaypoint()
waypoint.set_joint_angles(point)
self._motion.append_waypoint(waypoint)
print(self._motion.to_msg())
return self._motion.send_trajectory()
def move_to_joint_positions(self, positions):
self._motion.clear_waypoints()
self._motion.set_joint_names(positions.keys())
waypoint = MotionWaypoint()
waypoint.set_joint_angles(positions.values())
self._motion.append_waypoint(waypoint)
return self._motion.send_trajectory()