def sendWaypointTrajectory(self, waypoints, durations=0., vels=0., accs=0., effs=0.): if not self.ang_cmd_wait(waypoints[0]): print 'Cannot go to the first point in the trajectory' return None # else: # print 'Went to first' if not self.traj_connection: print 'Action server connection was not established' return None joint_traj = JointTrajectory() joint_traj.joint_names = self.arm_joint_names if not durations == 0: if not len(durations) == waypoints: raise Exception( 'The number of duration points is not equal to the number of provided waypoints' ) if not vels == 0: if not len(vels) == waypoints: raise Exception( 'The number velocity points is not equal to the number of provided waypoints' ) if not accs == 0: if not len(accs) == waypoints: raise Exception( 'The number acceleration points is not equal to the number of provided waypoints' ) if not effs == 0: if not len(effs) == waypoints: raise Exception( 'The number effort points is not equal to the number of provided waypoints' ) if not effs == 0: if not (vels == 0 and accs == 0): raise Exception( 'Cannot specify efforts with velocities and accelerations at the same time' ) if (not accs == 0) and vels == 0: raise Exception('Cannot specify accelerations without velocities') total_time_from_start = 0.5 for t in range(0, len(waypoints)): point = JointTrajectoryPoint() waypoint = waypoints[t] if not len(waypoint) == len(joint_traj.joint_names): raise Exception( 'The number of provided joint positions is not equal to the number of available joints for index: ' + str(t)) point.positions = waypoint if not vels == 0.: velocity = vels[t] if not len(velocity) == len(joint_traj.joint_names): raise Exception( 'The number of provided joint velocities is not equal to the number of available joints for index: ' + str(t)) point.velocities = velocity if not accs == 0.: acceleration = accs[t] if not len(acceleration) == len(joint_traj.joint_names): raise Exception( 'The number of provided joint accelerations is not equal to the number of available joints for index: ' + str(t)) point.accelerations = accelerations if not effs == 0.: effort = effs[t] if not len(effort) == len(joint_traj.joint_names): raise Exception( 'The number of provided joint efforts is not equal to the number of available joints for index: ' + str(t)) point.effort = effort if not durations == 0.: point.duration = duration # Deal with increasing time for each trajectory point point.time_from_start = rospy.Duration(total_time_from_start) total_time_from_start = total_time_from_start + 1.0 # Set the points joint_traj.points.append(point) traj_goal = FollowJointTrajectoryGoal() traj_goal.trajectory = joint_traj self.smooth_joint_trajectory_client.send_goal(traj_goal) self.smooth_joint_trajectory_client.wait_for_result() return self.smooth_joint_trajectory_client.get_result()
def sendWaypointTrajectory(self, waypoints, durations = 0., vels = 0., accs = 0., effs = 0.): if not self.ang_cmd_wait(waypoints[0]): print 'Cannot go to the first point in the trajectory' return None # else: # print 'Went to first' if not self.traj_connection: print 'Action server connection was not established' return None joint_traj = JointTrajectory() joint_traj.joint_names = self.arm_joint_names; if not durations == 0: if not len(durations) == waypoints: raise Exception('The number of duration points is not equal to the number of provided waypoints') if not vels == 0: if not len(vels) == waypoints: raise Exception('The number velocity points is not equal to the number of provided waypoints') if not accs == 0: if not len(accs) == waypoints: raise Exception('The number acceleration points is not equal to the number of provided waypoints') if not effs == 0: if not len(effs) == waypoints: raise Exception('The number effort points is not equal to the number of provided waypoints') if not effs == 0: if not (vels == 0 and accs == 0): raise Exception('Cannot specify efforts with velocities and accelerations at the same time') if (not accs == 0) and vels == 0: raise Exception('Cannot specify accelerations without velocities') total_time_from_start = 0.5; for t in range(0, len(waypoints)): point = JointTrajectoryPoint() waypoint = waypoints[t] if not len(waypoint) == len(joint_traj.joint_names): raise Exception('The number of provided joint positions is not equal to the number of available joints for index: ' + str(t)) point.positions = waypoint if not vels == 0.: velocity = vels[t] if not len(velocity) == len(joint_traj.joint_names): raise Exception('The number of provided joint velocities is not equal to the number of available joints for index: ' + str(t)) point.velocities = velocity if not accs == 0.: acceleration = accs[t] if not len(acceleration) == len(joint_traj.joint_names): raise Exception('The number of provided joint accelerations is not equal to the number of available joints for index: ' + str(t)) point.accelerations = accelerations if not effs == 0.: effort = effs[t] if not len(effort) == len(joint_traj.joint_names): raise Exception('The number of provided joint efforts is not equal to the number of available joints for index: ' + str(t)) point.effort = effort if not durations == 0.: point.duration = duration # Deal with increasing time for each trajectory point point.time_from_start = rospy.Duration(total_time_from_start) total_time_from_start = total_time_from_start + 1.0 # Set the points joint_traj.points.append(point) traj_goal = FollowJointTrajectoryGoal() traj_goal.trajectory = joint_traj self.smooth_joint_trajectory_client.send_goal(traj_goal) self.smooth_joint_trajectory_client.wait_for_result() return self.smooth_joint_trajectory_client.get_result()