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()
