00001
00002
00003 import roslib; roslib.load_manifest('r2_control')
00004 import rospy
00005 import actionlib
00006 import math
00007 import random
00008
00009 from control_msgs.msg import *
00010 from trajectory_msgs.msg import *
00011 from sensor_msgs.msg import JointState
00012 from copy import copy, deepcopy
00013
00014 TORAD = math.pi/180.0
00015 TODEG = 1.0/TORAD
00016
00017 class r2FullBodyReadyPose :
00018
00019 def __init__(self, N, wp, arm):
00020
00021 self.arm = arm
00022 self.currentData = None
00023 self.desiredData = None
00024 self.deadlineData = None
00025
00026 self.currentState = JointState()
00027 self.currentState.position = [0]*N
00028 self.currentState.velocity = [0]*N
00029 self.currentState.effort = [0]*N
00030 self.numJoints = N
00031 self.waypoints = wp
00032
00033 self.fingers = [("index",4),("middle",4),("ring",3),("little",3),("thumb",4)]
00034 rospy.Subscriber("r2/joint_states", JointState, self.jointStateCallback)
00035
00036 if self.arm=="left_arm" :
00037 self.trajPublisher = rospy.Publisher('/r2/l_arm_controller/command', JointTrajectory)
00038 self.trajClient = actionlib.SimpleActionClient('r2/l_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00039 elif self.arm=="right_arm" :
00040 self.trajPublisher = rospy.Publisher('/r2/r_arm_controller/command', JointTrajectory)
00041 self.trajClient = actionlib.SimpleActionClient('r2/r_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00042 elif self.arm=="left_leg" :
00043 self.trajPublisher = rospy.Publisher('/r2/l_leg_controller/command', JointTrajectory)
00044 self.trajClient = actionlib.SimpleActionClient('r2/l_leg_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00045 elif self.arm=="right_leg" :
00046 self.trajPublisher = rospy.Publisher('/r2/r_leg_controller/command', JointTrajectory)
00047 self.trajClient = actionlib.SimpleActionClient('r2/r_leg_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00048 elif self.arm=="left_hand" :
00049 self.trajPublisher = rospy.Publisher('/r2/l_hand_controller/command', JointTrajectory)
00050 self.trajClient = actionlib.SimpleActionClient('r2/l_hand_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00051 elif self.arm=="right_hand" :
00052 self.trajPublisher = rospy.Publisher('/r2/r_hand_controller/command', JointTrajectory)
00053 self.trajClient = actionlib.SimpleActionClient('r2/r_hand_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00054 elif self.arm=="left_foot" :
00055 self.trajPublisher = rospy.Publisher('/r2/l_foot_controller/command', JointTrajectory)
00056 self.trajClient = actionlib.SimpleActionClient('r2/l_foot_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00057 elif self.arm=="right_foot" :
00058 self.trajPublisher = rospy.Publisher('/r2/r_foot_controller/command', JointTrajectory)
00059 self.trajClient = actionlib.SimpleActionClient('r2/r_foot_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00060 elif self.arm=="neck" :
00061 self.trajPublisher = rospy.Publisher('/r2/neck_controller/command', JointTrajectory)
00062 self.trajClient = actionlib.SimpleActionClient('r2/neck_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
00063 else :
00064 rospy.logerr("r2FullBodyReadyPose::r2FullBodyReadyPose() -- unknown arm")
00065
00066 self.trajClient.wait_for_server()
00067
00068 self.actionGoal = FollowJointTrajectoryGoal()
00069
00070
00071 def getNumJoints(self) :
00072 return self.numJoints
00073
00074 def jointStateCallback(self, data):
00075 self.currentState = data
00076
00077 def computeTrajectory(self, desiredData, deadline):
00078
00079 jointTraj = JointTrajectory()
00080 currentState = copy(self.currentState)
00081 desiredState = copy(desiredData)
00082
00083
00084 rospy.loginfo("r2FullBodyReadyPose::computeTrajectory() -- finding necessary joints")
00085 desiredPositions = []
00086 currentPositions = []
00087 for desIndex in range(len(desiredState.name)) :
00088 for curIndex in range(len(currentState.name)) :
00089 if ( desiredState.name[desIndex] == currentState.name[curIndex] ) :
00090 desiredPositions.append(desiredState.position[desIndex])
00091 currentPositions.append(currentState.position[curIndex])
00092
00093 rospy.loginfo("r2FullBodyReadyPose::computeTrajectory() -- creating trajectory")
00094 jointTraj.joint_names = desiredState.name
00095 jointTraj.points = list()
00096
00097 for j in range(self.waypoints) :
00098 trajPoint = JointTrajectoryPoint()
00099
00100 t = (deadline / self.waypoints) * (j + 1)
00101 trajPoint.time_from_start = rospy.Duration(t)
00102
00103 trajPoint.positions = list()
00104 for i in range(len(desiredPositions)) :
00105 trajPoint.positions.append( self.minJerk(currentPositions[i], desiredPositions[i], deadline, t) )
00106
00107 jointTraj.points.append(trajPoint)
00108
00109 rospy.loginfo("r2FullBodyReadyPose::moveToGoal() -- using tolerances")
00110
00111 return jointTraj
00112
00113
00114 def minJerk(self, start, end, duration, t):
00115 tOverD = float(t) / float(duration)
00116 return start + (end - start)*( 10*math.pow(tOverD,3) - 15*math.pow(tOverD,4) + 6*math.pow(tOverD,5) )
00117
00118 def moveToGoal(self, jointGoal, deadline, useTolerances) :
00119
00120 self.actionGoal.trajectory = self.computeTrajectory(jointGoal, deadline)
00121
00122 offset = 0
00123
00124 if useTolerances :
00125 rospy.loginfo("r2FullBodyReadyPose::moveToGoal() -- using tolerances")
00126 self.actionGoal.path_tolerance = []
00127 self.actionGoal.goal_tolerance = []
00128
00129 if self.arm == "left_hand" :
00130 for k in range(len(self.fingers)):
00131 for j in range(self.fingers[k][1]):
00132 tol.name = "r2/left_arm/hand/" + self.fingers[k][0] + "/joint" + str(j+offset)
00133 tol.position = 0.2
00134 tol.velocity = 1
00135 tol.acceleration = 10
00136 self.actionGoal.path_tolerance.append(tol)
00137 self.actionGoal.goal_tolerance.append(tol)
00138 elif self.arm == "right_hand" :
00139 for k in range(len(self.fingers)):
00140 for i in range(self.fingers[k][1]):
00141 tol.name = "r2/right_arm/hand/" + self.fingers[k][0] + "/joint" + str(j+offset)
00142 print tol.name
00143 tol.position = 0.2
00144 tol.velocity = 1
00145 tol.acceleration = 10
00146 self.actionGoal.path_tolerance.append(tol)
00147 self.actionGoal.goal_tolerance.append(tol)
00148 else :
00149 for i in range(self.numJoints):
00150 tol = JointTolerance()
00151 if self.arm == "left_arm" or self.arm == "right_arm" :
00152 tol.name = "r2/" + self.arm + "/joint" + str(i+offset)
00153 elif self.arm == "left_leg" or self.arm == "right_leg" :
00154 tol.name = "r2/" + self.arm + "/joint" + str(i+offset)
00155 elif self.arm == "left_foot" :
00156 tol.name = "r2/left_leg/gripper/joint" + str(i+offset)
00157 elif self.arm == "right_foot" :
00158 tol.name = "r2/right_leg/gripper/joint" + str(i+offset)
00159 elif self.arm == "neck" :
00160 tol.name = "r2/" + self.arm + "/joint" + str(i+offset)
00161 tol.position = 0.2
00162 tol.velocity = 1
00163 tol.acceleration = 10
00164
00165 self.actionGoal.path_tolerance.append(tol)
00166 self.actionGoal.goal_tolerance.append(tol)
00167
00168 else :
00169 rospy.loginfo("r2FullBodyReadyPose::moveToGoal() -- not using tolerances")
00170
00171 self.actionGoal.goal_time_tolerance = rospy.Duration(10.0)
00172
00173
00174 self.trajClient.send_goal(self.actionGoal)
00175
00176 rospy.loginfo("r2FullBodyReadyPose::moveToGoal() -- returned state: %s", str(self.trajClient.get_state()))
00177 rospy.loginfo("r2FullBodyReadyPose::moveToGoal() -- returned result: %s", str(self.trajClient.get_result()))
00178
00179 return
00180
00181 def formatJointStateMsg(self, j, offset) :
00182
00183 if not (len(j) == self.numJoints) :
00184 rospy.logerr("r2FullBodyReadyPose::formatJointStateMsg() -- incorrectly sized joint message")
00185 return None
00186
00187 js = JointState()
00188 js.header.seq = 0
00189 js.header.stamp = rospy.Time.now()
00190 js.header.frame_id = ""
00191 js.name = []
00192 js.position = []
00193
00194
00195 if self.arm == "left_arm" or self.arm == "right_arm" :
00196 for i in range(self.numJoints):
00197 js.name.append("r2/" + self.arm + "/joint" + str(i+offset))
00198 js.position.append(j[i])
00199 if self.arm == "left_leg" or self.arm == "right_leg" :
00200 for i in range(self.numJoints):
00201 js.name.append("r2/" + self.arm + "/joint" + str(i+offset))
00202 js.position.append(j[i])
00203 if self.arm == "left_foot" :
00204 for i in range(self.numJoints):
00205 js.name.append("r2/left_leg/gripper/joint" + str(i+offset))
00206 js.position.append(j[i])
00207 if self.arm == "right_foot" :
00208 for i in range(self.numJoints):
00209 js.name.append("r2/right_leg/gripper/joint" + str(i+offset))
00210 js.position.append(j[i])
00211 if self.arm == "left_hand" :
00212 for k in range(len(self.fingers)):
00213 for i in range(self.fingers[k][1]):
00214 js.name.append("r2/left_arm/hand/" + self.fingers[k][0] + "/joint" + str(i+offset))
00215 js.position.append(j[i])
00216 if self.arm == "right_hand" :
00217 for k in range(len(self.fingers)):
00218 for i in range(self.fingers[k][1]):
00219 js.name.append("r2/right_arm/hand/" + self.fingers[k][0] + "/joint" + str(i+offset))
00220 js.position.append(j[i])
00221 elif self.arm == "neck" :
00222 for i in range(self.numJoints):
00223 js.name.append("r2/" + self.arm + "/joint" + str(i+offset))
00224 js.position.append(j[i])
00225
00226 return js
00227
00228
00229 if __name__ == '__main__':
00230 rospy.init_node('r2_ready_pose')
00231 try:
00232 r2TrajectoryGeneratorLeftArm = r2FullBodyReadyPose(7, 500, "left_arm")
00233 r2TrajectoryGeneratorRightArm = r2FullBodyReadyPose(7, 500, "right_arm")
00234 r2TrajectoryGeneratorLeftLeg = r2FullBodyReadyPose(7, 500, "left_leg")
00235 r2TrajectoryGeneratorRightLeg = r2FullBodyReadyPose(7, 500, "right_leg")
00236 r2TrajectoryGeneratorNeck = r2FullBodyReadyPose(3, 500, "neck")
00237 r2TrajectoryGeneratorLeftHand = r2FullBodyReadyPose(15, 10, "left_hand")
00238 r2TrajectoryGeneratorRightHand = r2FullBodyReadyPose(15, 10, "right_hand")
00239 r2TrajectoryGeneratorLeftFoot = r2FullBodyReadyPose(3, 10, "left_foot")
00240 r2TrajectoryGeneratorRightFoot = r2FullBodyReadyPose(3, 10, "right_foot")
00241 rospy.sleep(2)
00242
00243
00244 lhrp = [0]*15
00245 rhrp = [0]*15
00246
00247 lfrp = [0, -0.5, -0.5]
00248 rfrp = [0, -0.5, -0.5]
00249
00250 larp = [50.0*TORAD, -80.0*TORAD, -105.0*TORAD, -140.0*TORAD, 80.0*TORAD, 0.0*TORAD, 0.0*TORAD]
00251 rarp = [-50.0*TORAD, -80.0*TORAD, 105.0*TORAD, -140.0*TORAD, -80.0*TORAD, 0.0*TORAD, 0.0*TORAD]
00252
00253 llrp = [-1.2, 0.0, -2.0, 2.25, 0.8, 1.5, 2.5]
00254 rlrp = [1.2, 0.0, 2.0, 2.25, -0.8, 1.5, -2.5]
00255
00256 nrp = [-20.0*TORAD, 0.0*TORAD, -15.0*TORAD]
00257 print "r2FullBodyReadyPose() -- moving to ready pose"
00258
00259 jointGoalLeftArm = r2TrajectoryGeneratorLeftArm.formatJointStateMsg(larp, 0)
00260 jointGoalRightArm = r2TrajectoryGeneratorRightArm.formatJointStateMsg(rarp, 0)
00261 jointGoalLeftLeg = r2TrajectoryGeneratorLeftLeg.formatJointStateMsg(llrp, 0)
00262 jointGoalRightLeg = r2TrajectoryGeneratorRightLeg.formatJointStateMsg(rlrp, 0)
00263 jointGoalNeck = r2TrajectoryGeneratorNeck.formatJointStateMsg(nrp, 0)
00264 jointGoalLeftHand = r2TrajectoryGeneratorLeftHand.formatJointStateMsg(lhrp, 0)
00265 jointGoalRightHand = r2TrajectoryGeneratorRightHand.formatJointStateMsg(rhrp, 0)
00266 jointGoalLeftFoot = r2TrajectoryGeneratorLeftFoot.formatJointStateMsg(lfrp, 0)
00267 jointGoalRightFoot = r2TrajectoryGeneratorRightFoot.formatJointStateMsg(rfrp, 0)
00268
00269 r2TrajectoryGeneratorLeftArm.moveToGoal(jointGoalLeftArm, 0.5, False)
00270 r2TrajectoryGeneratorRightArm.moveToGoal(jointGoalRightArm, 0.5, False)
00271 r2TrajectoryGeneratorLeftLeg.moveToGoal(jointGoalLeftLeg, 0.5, False)
00272 r2TrajectoryGeneratorRightLeg.moveToGoal(jointGoalRightLeg, 0.5, False)
00273 r2TrajectoryGeneratorLeftHand.moveToGoal(jointGoalLeftHand, 0.1, False)
00274 r2TrajectoryGeneratorRightHand.moveToGoal(jointGoalRightHand, 0.1, False)
00275 r2TrajectoryGeneratorLeftFoot.moveToGoal(jointGoalLeftFoot, 0.1, False)
00276 r2TrajectoryGeneratorRightFoot.moveToGoal(jointGoalRightFoot, 0.1, False)
00277 r2TrajectoryGeneratorNeck.moveToGoal(jointGoalNeck, 0.5, False)
00278
00279 except rospy.ROSInterruptException:
00280 pass
00281
00282
00283
00284