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,