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()
pose_goal.orientation.z = OR_Z
pose_goal.position.x = BASE_X
pose_goal.position.y = BASE_Y
pose_goal.position.z = BASE_Z
poseStamped = PoseStamped()
poseStamped.pose = pose_goal
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
joint_angles = limb.joint_ordered_angles()
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
waypoint.set_cartesian_pose(poseStamped, 'right_hand', joint_angles)
traj.append_waypoint(waypoint.to_msg())
pub.publish(traj.to_msg())
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
rate = rospy.Rate(30.0)
counter = 0
while not rospy.is_shutdown():
try:
(trans, _) = listener.lookupTransform('kinect/user_1/torso',
'kinect/user_1/left_hand',
rospy.Time(0))
(arm_dir, _) = listener.lookupTransform('kinect/user_1/left_elbow',
'kinect/user_1/left_hand',
rospy.Time(0))
# (shoulder_dir, _) = listener.lookupTransform('kinect/user_1/left_shoulder', 'kinect/user_1/left_elbow', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,