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()
def run(self): rate = rospy.Rate(100) limb = Limb() traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options=traj_options, limb=limb) wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.5, max_joint_accel=0.5, corner_distance=0.05) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) self.pose.header = Header(stamp=rospy.Time.now(), frame_id='base') self.pose.pose.position.x = 0.0 self.pose.pose.position.y = -0.6 self.pose.pose.position.z = 0.5 self.pose.pose.orientation.x = 0.5 self.pose.pose.orientation.y = -0.5 self.pose.pose.orientation.z = 0.5 self.pose.pose.orientation.w = 0.5 joint_angles = limb.joint_ordered_angles() waypoint.set_cartesian_pose(self.pose, "right_hand", joint_angles) self.waypoints.append(waypoint) rospy.loginfo("Sending inital waypoint: %s", self.waypoints[0].to_string()) traj.append_waypoint(self.waypoints[0].to_msg()) result = traj.send_trajectory() if result is None: rospy.logerr('Trajectory FAILED to send') elif result.result: rospy.loginfo( 'Motion controller successfully finished the trajectory!') else: rospy.logerr( 'Motion controller failed to complete the trajectory with error %s', result.errorId) traj.clear_waypoints() l = Lights() l_name = 'head_green_light' initial_state = l.get_light_state(l_name) for i in range(0, 2): state = not initial_state l.set_light_state(l_name, state) rospy.sleep(0.5) state = not state l.set_light_state(l_name, state) rospy.sleep(0.5) l.set_light_state(l_name, True) for i in range(0, 19): self.gen_rand_waypoint() sendCommand = True while not rospy.is_shutdown(): traj.clear_waypoints() for i in range(0, 19): self.waypoints.pop(i) self.gen_rand_waypoint() for point in self.waypoints: traj.append_waypoint(point.to_msg()) print(len(self.waypoints)) result = traj.send_trajectory(wait_for_result=True) self.firstShutdown = True def clean_shutdown(): if self.firstShutdown: print("STOPPING TRAJECTORY") traj.stop_trajectory() traj.clear_waypoints() l = Lights() l.set_light_state('head_green_light', False) self.firstShutdown = False rospy.on_shutdown(clean_shutdown) rate.sleep() return
pose_goal.orientation.w = OR_W pose_goal.orientation.x = OR_X pose_goal.orientation.y = OR_Y pose_goal.orientation.z = OR_Z trans = [ -SCALING * trans[2] + BASE_X, -SCALING * trans[0] + BASE_Y, SCALING * trans[1] + BASE_Z ] pose_goal.position.x = trans[0] pose_goal.position.y = trans[1] pose_goal.position.z = trans[2] poseStamped = PoseStamped() poseStamped.pose = pose_goal 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()) if (counter >= 30): pub.publish(traj.to_msg()) traj.clear_waypoints() counter = 0 rate.sleep()