sr_example: sr_right_hand_partial_traj

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import numpy as np
00004 
00005 import rospy
00006 from control_msgs.msg import JointTrajectoryControllerState, \
00007     FollowJointTrajectoryActionResult, FollowJointTrajectoryActionGoal
00008 import matplotlib.pyplot as plt
00009 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00010 
00011 from sr_robot_commander.sr_hand_commander import SrHandCommander
00012 
00013 
00014 class PartialTrajListener():
00015     def __init__(self):
00016         self.start_time_goals = []
00017         self.start_time_goals_trajectory = []
00018         self.goal_joint_names = []
00019         self.trajectories = []
00020         self.start_goals = False
00021         self.finish_goals = False
00022 
00023         self.joints_time = []
00024         self.ffj3_actual = []
00025         self.ffj3_desired = []
00026         self.ffj3_error = []
00027         self.mfj3_actual = []
00028         self.mfj3_desired = []
00029         self.mfj3_error = []
00030         self.ffj3_vel_actual = []
00031         self.mfj3_vel_actual = []
00032         self.ffj3_vel_desired = []
00033         self.mfj3_vel_desired = []
00034         self.ffj3_vel_error = []
00035         self.mfj3_vel_error = []
00036 
00037         rospy.Subscriber("/rh_trajectory_controller/state",
00038                          JointTrajectoryControllerState, self.callback)
00039         rospy.Subscriber(
00040             "/rh_trajectory_controller/follow_joint_trajectory/result",
00041             FollowJointTrajectoryActionResult, self.callback_result)
00042         rospy.Subscriber(
00043             "/rh_trajectory_controller/follow_joint_trajectory/goal",
00044             FollowJointTrajectoryActionGoal, self.callback_goal)
00045 
00046     def callback(self, state):
00047         self.joint_names = state.joint_names
00048         self.ffj3_index = self.joint_names.index("rh_FFJ3")
00049         self.mfj3_index = self.joint_names.index("rh_MFJ3")
00050         if self.start_goals and not self.finish_goals:
00051             self.joints_time.append(state.header.stamp.to_sec())
00052             self.ffj3_actual.append(state.actual.positions[self.ffj3_index])
00053             self.ffj3_desired.append(state.desired.positions[self.ffj3_index])
00054             self.ffj3_error.append(state.error.positions[self.ffj3_index])
00055             self.mfj3_actual.append(state.actual.positions[self.mfj3_index])
00056             self.mfj3_desired.append(state.desired.positions[self.mfj3_index])
00057             self.mfj3_error.append(state.error.positions[self.mfj3_index])
00058 
00059             self.ffj3_vel_actual.append(
00060                 state.actual.velocities[self.ffj3_index])
00061             self.mfj3_vel_actual.append(
00062                 state.actual.velocities[self.mfj3_index])
00063             self.ffj3_vel_desired.append(
00064                 state.desired.velocities[self.ffj3_index])
00065             self.mfj3_vel_desired.append(
00066                 state.desired.velocities[self.mfj3_index])
00067             self.ffj3_vel_error.append(state.error.velocities[self.ffj3_index])
00068             self.mfj3_vel_error.append(state.error.velocities[self.mfj3_index])
00069 
00070     def callback_result(self, result):
00071         print ("Trajectory Goal:" + result.status.goal_id.id +
00072                " finished with status:" + str(result.status.status))
00073 
00074     def callback_goal(self, goal):
00075         self.start_goals = True
00076         self.goal_joint_names.append(goal.goal.trajectory.joint_names)
00077         self.start_time_goals.append(goal.header.stamp.to_sec())
00078         self.start_time_goals_trajectory.append(
00079             goal.goal.trajectory.header.stamp.to_sec())
00080         self.trajectories.append(goal.goal.trajectory.points)
00081 
00082     def plot_settings(self, plt):
00083         ax = plt.gca()
00084         plt.grid(which='both', axis='both')
00085         plt.setp(ax.get_xticklabels(), fontsize=8)
00086         plt.setp(ax.get_yticklabels(), fontsize=8)
00087         plt.xlabel('Time (s)')
00088         ax.xaxis.label.set_size(10)
00089         ax.yaxis.label.set_size(10)
00090 
00091     def graph(self):
00092         time_cero = self.joints_time[0]
00093         time = np.array(self.joints_time) - time_cero
00094 
00095         plt.figure()
00096 
00097         # Plot goal trajectories waypoints
00098         time_ffj3_traj = []
00099         angle_ffj3_traj = []
00100         time_mfj3_traj = []
00101         angle_mfj3_traj = []
00102         for i, traj in enumerate(self.trajectories):
00103             ffj3_goal_index = self.goal_joint_names[i].index(
00104                 "rh_FFJ3") if "rh_FFJ3" in self.goal_joint_names[i] else -1
00105             mfj3_goal_index = self.goal_joint_names[i].index(
00106                 "rh_MFJ3") if "rh_MFJ3" in self.goal_joint_names[i] else -1
00107 
00108             for point in traj:
00109                 if ffj3_goal_index > -1:
00110                     time_ffj3_traj.append(point.time_from_start.to_sec() +
00111                                           self.start_time_goals_trajectory[i] -
00112                                           time_cero)
00113                     angle_ffj3_traj.append(point.positions[ffj3_goal_index])
00114                 if mfj3_goal_index > -1:
00115                     time_mfj3_traj.append(point.time_from_start.to_sec() +
00116                                           self.start_time_goals_trajectory[i] -
00117                                           time_cero)
00118                     angle_mfj3_traj.append(point.positions[mfj3_goal_index])
00119 
00120             if ffj3_goal_index > -1:
00121                 plt.subplot(3, 2, 1)
00122                 plt.plot(time_ffj3_traj, angle_ffj3_traj, 'o',
00123                          label="Traj " + str(i + 1))
00124 
00125             if mfj3_goal_index > -1:
00126                 plt.subplot(3, 2, 2)
00127                 plt.plot(time_mfj3_traj, angle_mfj3_traj, 'o',
00128                          label="Traj " + str(i + 1))
00129 
00130             time_ffj3_traj = []
00131             angle_ffj3_traj = []
00132             time_mfj3_traj = []
00133             angle_mfj3_traj = []
00134 
00135         # Plot trajectories
00136         plt.subplot(3, 2, 1)
00137         plt.plot(time, self.ffj3_actual, 'black', label="Actual traj")
00138         plt.plot(time, self.ffj3_desired, 'green', label="Desired traj")
00139         plt.ylabel('ffj3 Actual position (rad)')
00140         self.plot_settings(plt)
00141         plt.ylim(ymax=2.2, ymin=-0.1)
00142         plt.legend(bbox_to_anchor=(0., 1.02, 1., .102), loc=3, ncol=5,
00143                    mode="expand", borderaxespad=0., prop={'size': 8})
00144 
00145         plt.subplot(3, 2, 2)
00146         plt.plot(time, self.mfj3_actual, 'black', label="Actual traj")
00147         plt.plot(time, self.mfj3_desired, 'green', label="Desired traj")
00148         plt.ylabel('mfj3 Actual position (rad)')
00149         self.plot_settings(plt)
00150         plt.ylim(ymax=2.2, ymin=-0.1)
00151         plt.legend(bbox_to_anchor=(0., 1.02, 1., .102), loc=3, ncol=4,
00152                    mode="expand", borderaxespad=0., prop={'size': 8})
00153 
00154         plt.subplot(3, 2, 3)
00155         plt.plot(time, self.ffj3_vel_actual, 'black', label="Actual traj")
00156         plt.plot(time, self.ffj3_vel_desired, 'green', label="Desired traj")
00157         plt.legend(bbox_to_anchor=(0., 1.02, 1., .102), loc=3, ncol=4,
00158                    mode="expand", borderaxespad=0., prop={'size': 8})
00159 
00160         plt.ylabel('ffj3 Actual velocity')
00161         plt.xlim(xmax=16, xmin=0)
00162         self.plot_settings(plt)
00163         plt.legend(bbox_to_anchor=(0., 1.02, 1., .102), loc=3, ncol=4,
00164                    mode="expand", borderaxespad=0., prop={'size': 8})
00165 
00166         plt.subplot(3, 2, 4)
00167         plt.plot(time, self.mfj3_vel_actual, 'black', label="Actual traj")
00168         plt.plot(time, self.mfj3_vel_desired, 'green', label="Desired traj")
00169         plt.ylabel('mfj3 Actual velocity')
00170         plt.xlim(xmax=16, xmin=0)
00171         self.plot_settings(plt)
00172         plt.legend(bbox_to_anchor=(0., 1.02, 1., .102), loc=3, ncol=4,
00173                    mode="expand", borderaxespad=0., prop={'size': 8})
00174 
00175         plt.subplot(3, 2, 5)
00176         plt.plot(time, self.ffj3_vel_error, 'red', label="Error traj")
00177         plt.ylabel('ffj3 Velocity Error')
00178         plt.xlim(xmax=16, xmin=0)
00179         self.plot_settings(plt)
00180 
00181         plt.subplot(3, 2, 6)
00182         plt.plot(time, self.mfj3_vel_error, 'red', label="Error traj")
00183         plt.ylabel('mfj3 Velocity Error')
00184         plt.xlim(xmax=16, xmin=0)
00185         self.plot_settings(plt)
00186 
00187         plt.subplots_adjust(left=0.07, right=0.96, bottom=0.083, top=0.90)
00188         plt.show()
00189 
00190 
00191 def construct_trajectory_point(posture, duration):
00192     trajectory_point = JointTrajectoryPoint()
00193     trajectory_point.time_from_start = rospy.Duration.from_sec(float(duration))
00194     for key in joint_trajectory.joint_names:
00195         trajectory_point.positions.append(posture[key])
00196     return trajectory_point
00197 
00198 
00199 open_hand = {'rh_FFJ1': 0.0, 'rh_FFJ2': 0.0, 'rh_FFJ3': 0.0, 'rh_FFJ4': 0.0,
00200              'rh_MFJ1': 0.0, 'rh_MFJ2': 0.0, 'rh_MFJ3': 0.0, 'rh_MFJ4': 0.0,
00201              'rh_RFJ1': 0.0, 'rh_RFJ2': 0.0, 'rh_RFJ3': 0.0, 'rh_RFJ4': 0.0,
00202              'rh_LFJ1': 0.0, 'rh_LFJ2': 0.0, 'rh_LFJ3': 0.0, 'rh_LFJ4': 0.0,
00203              'rh_LFJ5': 0.0, 'rh_THJ1': 0.0, 'rh_THJ2': 0.0, 'rh_THJ3': 0.0,
00204              'rh_THJ4': 0.0, 'rh_THJ5': 0.0, 'rh_WRJ1': 0.0, 'rh_WRJ2': 0.0}
00205 
00206 grasp1 = {'rh_FFJ1': 0.0, 'rh_FFJ2': 0.0, 'rh_FFJ3': 0.5235, 'rh_FFJ4': 0.0,
00207           'rh_MFJ1': 0.0, 'rh_MFJ2': 0.0, 'rh_MFJ3': 0.5235, 'rh_MFJ4': 0.0,
00208           'rh_RFJ1': 0.0, 'rh_RFJ2': 0.0, 'rh_RFJ3': 0.0, 'rh_RFJ4': 0.0,
00209           'rh_LFJ1': 0.0, 'rh_LFJ2': 0.0, 'rh_LFJ3': 0.0, 'rh_LFJ4': 0.0,
00210           'rh_LFJ5': 0.0, 'rh_THJ1': 0.0, 'rh_THJ2': 0.0, 'rh_THJ3': 0.0,
00211           'rh_THJ4': 0.0, 'rh_THJ5': 0.0, 'rh_WRJ1': 0.0, 'rh_WRJ2': 0.0}
00212 
00213 grasp2 = {'rh_FFJ1': 0.0, 'rh_FFJ2': 0.0, 'rh_FFJ3': 1.0472, 'rh_FFJ4': 0.0,
00214           'rh_MFJ1': 0.0, 'rh_MFJ2': 0.0, 'rh_MFJ3': 1.0472, 'rh_MFJ4': 0.0,
00215           'rh_RFJ1': 0.0, 'rh_RFJ2': 0.0, 'rh_RFJ3': 0.0, 'rh_RFJ4': 0.0,
00216           'rh_LFJ1': 0.0, 'rh_LFJ2': 0.0, 'rh_LFJ3': 0.0, 'rh_LFJ4': 0.0,
00217           'rh_LFJ5': 0.0, 'rh_THJ1': 0.0, 'rh_THJ2': 0.0, 'rh_THJ3': 0.0,
00218           'rh_THJ4': 0.0, 'rh_THJ5': 0.0, 'rh_WRJ1': 0.0, 'rh_WRJ2': 0.0}
00219 
00220 grasp3 = {'rh_FFJ1': 0.0, 'rh_FFJ2': 0.0, 'rh_FFJ3': 1.4, 'rh_FFJ4': 0.0,
00221           'rh_MFJ1': 0.0, 'rh_MFJ2': 0.0, 'rh_MFJ3': 1.4, 'rh_MFJ4': 0.0,
00222           'rh_RFJ1': 0.0, 'rh_RFJ2': 0.0, 'rh_RFJ3': 0.0, 'rh_RFJ4': 0.0,
00223           'rh_LFJ1': 0.0, 'rh_LFJ2': 0.0, 'rh_LFJ3': 0.0, 'rh_LFJ4': 0.0,
00224           'rh_LFJ5': 0.0, 'rh_THJ1': 0.0, 'rh_THJ2': 0.0, 'rh_THJ3': 0.0,
00225           'rh_THJ4': 0.0, 'rh_THJ5': 0.0, 'rh_WRJ1': 0.0, 'rh_WRJ2': 0.0}
00226 
00227 grasp4 = {'rh_FFJ1': 0.0, 'rh_FFJ2': 0.0, 'rh_FFJ3': 1.5, 'rh_FFJ4': 0.0,
00228           'rh_MFJ1': 0.0, 'rh_MFJ2': 0.0, 'rh_MFJ3': 1.5, 'rh_MFJ4': 0.0,
00229           'rh_RFJ1': 0.0, 'rh_RFJ2': 0.0, 'rh_RFJ3': 0.0, 'rh_RFJ4': 0.0,
00230           'rh_LFJ1': 0.0, 'rh_LFJ2': 0.0, 'rh_LFJ3': 0.0, 'rh_LFJ4': 0.0,
00231           'rh_LFJ5': 0.0, 'rh_THJ1': 0.0, 'rh_THJ2': 0.0, 'rh_THJ3': 0.0,
00232           'rh_THJ4': 0.0, 'rh_THJ5': 0.0, 'rh_WRJ1': 0.0, 'rh_WRJ2': 0.0}
00233 
00234 grasp5 = {'rh_FFJ1': 0.0, 'rh_FFJ2': 0.0, 'rh_FFJ3': 1.5707, 'rh_FFJ4': 0.0,
00235           'rh_MFJ1': 0.0, 'rh_MFJ2': 0.0, 'rh_MFJ3': 1.5707, 'rh_MFJ4': 0.0,
00236           'rh_RFJ1': 0.0, 'rh_RFJ2': 0.0, 'rh_RFJ3': 0.0, 'rh_RFJ4': 0.0,
00237           'rh_LFJ1': 0.0, 'rh_LFJ2': 0.0, 'rh_LFJ3': 0.0, 'rh_LFJ4': 0.0,
00238           'rh_LFJ5': 0.0, 'rh_THJ1': 0.0, 'rh_THJ2': 0.0, 'rh_THJ3': 0.0,
00239           'rh_THJ4': 0.0, 'rh_THJ5': 0.0, 'rh_WRJ1': 0.0, 'rh_WRJ2': 0.0}
00240 
00241 grasp_partial_1 = {'rh_FFJ3': 1.06}
00242 grasp_partial_2 = {'rh_FFJ3': 1.2}
00243 
00244 keys = ['rh_FFJ1', 'rh_FFJ2', 'rh_FFJ3', 'rh_FFJ4', 'rh_THJ4', 'rh_THJ5',
00245         'rh_THJ1', 'rh_THJ2', 'rh_THJ3', 'rh_LFJ2', 'rh_LFJ3', 'rh_LFJ1',
00246         'rh_LFJ4', 'rh_LFJ5', 'rh_RFJ4', 'rh_RFJ1', 'rh_RFJ2', 'rh_RFJ3',
00247         'rh_MFJ1', 'rh_MFJ3', 'rh_MFJ2', 'rh_MFJ4', 'rh_WRJ2', 'rh_WRJ1']
00248 
00249 if __name__ == '__main__':
00250     rospy.init_node("partial_traj_listener", anonymous=True)
00251     rospy.sleep(1)  # Do not start with zero
00252 
00253     listener = PartialTrajListener()
00254     hand_commander = SrHandCommander()
00255     start_time = rospy.Time.now()
00256 
00257     # Opening hand
00258     trajectory_start_time = 1.0
00259     joint_trajectory = JointTrajectory()
00260     joint_trajectory.header.stamp = start_time + rospy.Duration.from_sec(
00261         float(trajectory_start_time))
00262     joint_trajectory.joint_names = list(open_hand.keys())
00263     joint_trajectory.points = []
00264     trajectory_point = construct_trajectory_point(open_hand, 1.0)
00265     joint_trajectory.points.append(trajectory_point)
00266     hand_commander.run_joint_trajectory_unsafe(joint_trajectory, True)
00267 
00268     # Closing index and middle fingers
00269     trajectory_start_time = 4.0
00270     joint_trajectory = JointTrajectory()
00271     joint_trajectory.header.stamp = start_time + rospy.Duration.from_sec(
00272         float(trajectory_start_time))
00273     joint_trajectory.joint_names = list(grasp1.keys())
00274     joint_trajectory.points = []
00275     trajectory_point = construct_trajectory_point(grasp1, 1.0)
00276     joint_trajectory.points.append(trajectory_point)
00277     trajectory_point = construct_trajectory_point(grasp2, 4.0)
00278     joint_trajectory.points.append(trajectory_point)
00279     trajectory_point = construct_trajectory_point(grasp3, 6.0)
00280     joint_trajectory.points.append(trajectory_point)
00281     trajectory_point = construct_trajectory_point(grasp4, 8.0)
00282     joint_trajectory.points.append(trajectory_point)
00283     trajectory_point = construct_trajectory_point(grasp5, 10.0)
00284     joint_trajectory.points.append(trajectory_point)
00285     hand_commander.run_joint_trajectory_unsafe(joint_trajectory, False)
00286 
00287     # Stopping trajectory of index using a partial trajectory
00288     rospy.sleep(2)
00289     trajectory_start_time = 8.0
00290     joint_trajectory = JointTrajectory()
00291     joint_trajectory.header.stamp = start_time + rospy.Duration.from_sec(
00292         float(trajectory_start_time))
00293     joint_trajectory.joint_names = list(grasp_partial_1.keys())
00294     joint_trajectory.points = []
00295     trajectory_point = construct_trajectory_point(grasp_partial_1, 1.0)
00296     joint_trajectory.points.append(trajectory_point)
00297     trajectory_point = construct_trajectory_point(grasp_partial_2, 3.0)
00298     joint_trajectory.points.append(trajectory_point)
00299     hand_commander.run_joint_trajectory_unsafe(joint_trajectory, False)
00300 
00301     graphs_finished = False
00302     rate = rospy.Rate(10)
00303     while not rospy.is_shutdown():
00304         if len(listener.joints_time) > 5 and\
00305                 (listener.joints_time[-1] - listener.joints_time[0]) > 15 and\
00306                 not graphs_finished:
00307             listener.finish_goals = True
00308             listener.graph()
00309             graphs_finished = True
00310             break
00311         rate.sleep()