def is_valid(self,config,pname): if config == '': print "Hey: No IK Solution found! Move the robot!" return 0 else: print str2num(config) print type(str2num(config)) self.robot.SetActiveDOFValues(str2num(config)) if(self.env.CheckCollision(self.robot,self.crankid)): print "Point is in collision." return 0 else: print "Point "+pname+" validated" print "------" return 1
def go_to_start_config_and_pose(myRobot, myRmaps, sol, T0_starts): for myManipulatorIndex in range(2): # Find where to move the base startSphereIndex = sol[myManipulatorIndex][0].sIdx startTransformIndex = sol[myManipulatorIndex][0].tIdx myRmaps[myManipulatorIndex].go_to(startSphereIndex,startTransformIndex) Tbase_start = myRmaps[myManipulatorIndex].map[startSphereIndex].T[startTransformIndex] T0_newManipPose = dot(T0_starts[myManipulatorIndex],linalg.inv(Tbase_start)) myRobot.SetTransform(array(T0_newManipPose)) startConfigStr = sol[2] myRobot.SetActiveDOFValues(str2num(startConfigStr)) time.sleep(0.05)
def generateChain(self, offsetHandleToHand, handlePose, Bw0, Bw1, furnitureName, jointIndex, cbirrtProb): jointtm = str2num( cbirrtProb.SendCommand('GetJointTransform name ' + furnitureName + ' jointind %d' % jointIndex)) T0_w0 = MakeTransform(handlePose[:3][:, 0:3], mat(jointtm[9:12]).T) Tw1_e = offsetHandleToHand Tw0_e = linalg.inv(T0_w0) * handlePose TSRstring1 = SerializeTSR(0, 'NULL', T0_w0, Tw0_e, Bw0) TSRstring2 = SerializeTSR(0, 'NULL', mat(eye(4)), Tw1_e, Bw1) TSRChainString = SerializeTSRChain(0, 0, 1, 2, TSRstring1 + ' ' + TSRstring2, furnitureName, mat(jointIndex)) return TSRChainString
def go_to_start_config_and_pose(myRobot, myRmaps, sol, T0_starts): for myManipulatorIndex in range(2): # Find where to move the base startSphereIndex = sol[myManipulatorIndex][0].sIdx startTransformIndex = sol[myManipulatorIndex][0].tIdx myRmaps[myManipulatorIndex].go_to(startSphereIndex, startTransformIndex) Tbase_start = myRmaps[myManipulatorIndex].map[startSphereIndex].T[ startTransformIndex] T0_newManipPose = dot(T0_starts[myManipulatorIndex], linalg.inv(Tbase_start)) myRobot.SetTransform(array(T0_newManipPose)) startConfigStr = sol[2] myRobot.SetActiveDOFValues(str2num(startConfigStr)) time.sleep(0.05)
def is_valid(self, config, pname): if config == "": # print"Hey: No IK Solution found! Move the robot!" return 0 else: # printstr2num(config) # printtype(str2num(config)) self.robot.SetActiveDOFValues(str2num(config)) # sys.stdin.readline() if self.env.CheckCollision(self.robot, self.crankid): print "Point " + pname + " is in collision." return 0 else: print "Point " + pname + " is valid" # print"------" return 1
def test_points(self): self.robot.SetActiveDOFValues(str2num(self.approachik_1)) self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.approachik_1),self.robot.GetActiveDOFIndices()) print "went to approach-1" sys.stdin.readline() self.robot.SetActiveDOFValues(str2num(self.approachik_2)) self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.approachik_2),self.robot.GetActiveDOFIndices()) print "went to approach-2" sys.stdin.readline() self.robot.SetActiveDOFValues(str2num(self.startik)) self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.startik),self.robot.GetActiveDOFIndices()) print "went to startik" sys.stdin.readline() self.robot.SetActiveDOFValues(str2num(goalik)) # Note: self.goalik is T0_LH2 and T0_RH2 self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.goalik),self.robot.GetActiveDOFIndices()) print "went to goalik" sys.stdin.readline() self.robot.SetActiveDOFValues(str2num(startik)) self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.startik),self.robot.GetActiveDOFIndices()) ################################################ ## YOU'RE AT STARTIK - GO BACK TO APPROACH 2 ## ################################################ goaljoints = str2num(self.approachik_2); print "Press enter to go to approach point 2" sys.stdin.readline() self.SetRobotDOFValuesInSyncWithROS(self.robot,goaljoints,self.robot.GetActiveDOFIndices()) ################################################## ## YOU'RE AT APPROACH 2 - GO BACK TO APPROACH 1 ## ################################################## goaljoints = str2num(self.approachik_1); print "Press enter to go to approach point 1" sys.stdin.readline() self.SetRobotDOFValuesInSyncWithROS(self.robot,goaljoints,self.robot.GetActiveDOFIndices()) ###################################################### ## YOU'RE AT APPROACH 1 - GO BACK TO INITCONFIG ## ###################################################### print "Press enter to go to initconfig" sys.stdin.readline() self.SetRobotDOFValuesInSyncWithROS(self.robot,self.initconfig,self.robot.GetActiveDOFIndices()) return 1
class DrcHuboWheelTurning(BaseWheelTurning): def __init__( self, HuboModelPath='../../openHubo/drchubo/robots/drchubo.robot.xml', WheelModelPath='../../../drc_common/models/driving_wheel.robot.xml' ): BaseWheelTurning.__init__(self, HuboModelPath, WheelModelPath) # Set those variables to show or hide the interface # Do it using the member functions self.StopAtKeyStrokes = False self.ShowUserInterface = False self.ViewerStarted = False # Right Hand Joints # Open - Closed Values self.rhanddofs = range(26, 29) self.rhandclosevals = [0.95, 0.95, -0.95] self.rhandopenvals = [0, 0, 0] # Left Hand Joints self.lhanddofs = range(8, 11) self.lhandclosevals = [0.95, 0.95, -0.95] self.lhandopenvals = [0, 0, 0] def Run(self): self.RemoveFiles() # This is a list of handles of the objects that are # drawn on the screen in OpenRAVE Qt-Viewer. # Keep appending to the end, and pop() if you want to delete. handles = [] normalsmoothingitrs = 150 fastsmoothingitrs = 20 self.StartViewerAndSetWheelPos(handles) # Wheel Joint Index crankjointind = 0 # Set the wheel joints back to 0 for replanning self.crankid.SetDOFValues([0], [crankjointind]) self.crankid.GetController().Reset(0) shiftUp = 0.95 # Move the wheel infront of the robot self.crankid.SetTransform( array( MakeTransform( dot(rodrigues([0, 0, pi / 2]), rodrigues([pi / 2, 0, 0])), transpose(matrix([0.5, 0.0, shiftUp]))))) probs_cbirrt = RaveCreateModule(self.env, 'CBiRRT') probs_crankmover = RaveCreateModule(self.env, 'CBiRRT') manips = self.robotid.GetManipulators() crankmanip = self.crankid.GetManipulators() try: self.env.AddModule(probs_cbirrt, self.robotid.GetName()) self.env.AddModule(probs_crankmover, self.crankid.GetName()) except openrave_exception, e: print e print "Getting Loaded Problems" probs = self.env.GetLoadedProblems() # Keep Active Joint Indices activedofs = [0] for m in manips: activedofs.extend(m.GetArmIndices()) # Sort Active Joint Indices activedofs.sort() # elbows: Left Elbow Pitch: 4; Right Elbow Pitch: 22 self.robotid.SetDOFValues([-0.95, -0.95], [4, 22]) self.robotid.SetActiveDOFs(activedofs) # Current configuration of the robot is its initial configuration initconfig = self.robotid.GetActiveDOFValues() # List of Robot Links links = self.robotid.GetLinks() # List of Wheel (Crank Links) cranklinks = self.crankid.GetLinks() # End Effector Transforms Tee = [] for i in range(len(manips)): # Returns End Effector Transform in World Coordinates Tlink = manips[i].GetEndEffectorTransform() Tee.append(Tlink) # Wheel Joint Index crankjointind = 0 # Get Transformation Matrix for the Wheel # Note that crank's links are not rotated # If you want use the wheel's end effector's transformation # matrix (which is 23 degrees tilted) then see # CTee matrix below. # # crank has two links: # 0) pole - the blue cylinder in the model, and, # 1) crank - the driving wheel itself. jointtm = cranklinks[0].GetTransform() # handles.append(misc.DrawAxes(env,matrix(jointtm),1)) # We can also get the transformation matrix # with the following command as a string jointtm_str = probs[0].SendCommand( 'GetJointTransform name crank jointind ' + str(crankjointind)) # And then we can convert the string to a 1x12 array jointtm_str = jointtm_str.replace(" ", ",") jointtm_num = eval('[' + jointtm_str + ']') # In this script we will use jointtm. # jointtm_str and jointtm_num are given as example. # Crank Transform End Effector in World Coordinates # This is the transformation matrix of the end effector # named "dummy" in the xml file. # Note that dummy is tilted 23 degress around its X-Axis CTee = crankmanip[0].GetEndEffectorTransform() tilt_angle_deg = acos(dot(linalg.inv(CTee), jointtm)[1, 1]) * 180 / pi tilt_angle_rad = acos(dot(linalg.inv(CTee), jointtm)[1, 1]) # Center of Gravity Target # cogtarg = [-0.05, 0.085, 0] cogtarg = [0, 0, 0] # polyscale: changes the scale of the support polygon # polytrans: shifts the support polygon around footlinknames = ' Body_RAR Body_LAR ' #polytrans -0.015 0 0.0 ' # What is this? handrot = rodrigues([0, -pi / 2, 0]) # Translation Offset from the wheel center for the hands transoffset = [0, 0.15, 0] # Figure out where to put the left hand on the wheel temp = dot( CTee, MakeTransform(rodrigues([0, -pi / 2, 0]), transpose(matrix([0, 0, 0])))) temp = dot( temp, MakeTransform(rodrigues([-pi, 0, 0]), transpose(matrix([0, 0, 0])))) # Left Hand Pose in World Coordinates T0_LH1 = dot( temp, MakeTransform(rodrigues([0, 0, 0]), transpose(matrix([0, 0, 0.145])))) # Uncomment if you want to see where T0_LH1 is handles.append(misc.DrawAxes(self.env, matrix(T0_LH1), 1)) # Figure out where to put the right hand on the wheel temp = dot( CTee, MakeTransform(rodrigues([0, -pi / 2, 0]), transpose(matrix([0, 0, 0])))) # Right Hand Pose in World Coordinates T0_RH1 = dot( temp, MakeTransform(rodrigues([0, 0, 0]), transpose(matrix([0, 0, 0.145])))) # Uncomment if you want to see where T0_RH1 is handles.append(misc.DrawAxes(self.env, matrix(T0_RH1), 1)) # Define Task Space Region strings # Left Hand TSRString1 = SerializeTSR(0, 'NULL', T0_LH1, eye(4), matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])) # Right Hand TSRString2 = SerializeTSR(1, 'NULL', T0_RH1, eye(4), matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])) # Left Foot TSRString3 = SerializeTSR(2, 'NULL', Tee[2], eye(4), matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])) # Right Foot TSRString4 = SerializeTSR(3, 'NULL', Tee[3], eye(4), matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])) # Head # Grasp transform in Head coordinates Tw0_eH = eye(4) # How much freedom do we want to give to the Head # [x,x,y,y,z,z,R,R,P,P,Y,Y] Bw0H = matrix( [-0.05, 0.05, -0.1, 0.1, -100, 100, -pi, pi, -pi, pi, -pi, pi]) TSRString5 = SerializeTSR(4, 'NULL', Tee[4], Tw0_eH, Bw0H) # We defined Task Space Regions. Now let's concatenate them. TSRChainStringGrasping = SerializeTSRChain( 0, 1, 0, 1, TSRString1, 'NULL', []) + ' ' + SerializeTSRChain( 0, 1, 0, 1, TSRString2, 'NULL', []) + ' ' + SerializeTSRChain( 0, 1, 1, 1, TSRString3, 'NULL', []) + ' ' + SerializeTSRChain( 0, 1, 1, 1, TSRString4, 'NULL', [] ) #+' '+SerializeTSRChain(0,1,0,1,TSRString5,'NULL',[]) if (self.StopAtKeyStrokes): print "Press Enter to go to startik" sys.stdin.readline() startik = probs[0].SendCommand( 'DoGeneralIK exec nummanips 2 maniptm 0 ' + trans_to_str(T0_LH1) + ' maniptm 1 ' + trans_to_str(T0_RH1)) # Rotate the wheel's transform to a suitable pose # for the Left Hand # T0_w0L stands for: # left hand's transform on wheel in world coordinates T0_w0L = MakeTransform(rodrigues([0, tilt_angle_rad, 0]), transpose(matrix(jointtm[0:3, 3]))) # This is what's happening: # # Tw0L_0 = linalg.inv(T0_w0L) # Tw0L_LH1 = Tw0L_0*T0_LH1 # # Left hand's transform in wheel's coordinates Tw0L_LH1 = dot(linalg.inv(T0_w0L), T0_LH1) # Transform of the left hand's end effector in wheel's coords. # Required by CBiRRT Tw0_eL = Tw0L_LH1 # How much freedom do we want to give to the left hand Bw0L = matrix([0, 0, 0, 0, 0, 0, 0, pi, 0, 0, 0, 0]) # Right Hand's transforms: T0_crankcrank = self.crankid.GetManipulators()[0].GetTransform() T0_w0R = MakeTransform(rodrigues([tilt_angle_rad, 0, 0]), transpose(matrix([0, 0, 0]))) # End effector transform in wheel coordinates Tw0_eR = dot(linalg.inv(T0_crankcrank), T0_RH1) # handles.append(misc.DrawAxes(env,matrix(Tw0_eR),1)) # How much freedom? (note: in frame of crank) Bw0R = matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) # Head's transforms: T0_w0H = Tee[4] Tw0_eH = eye(4) Bw0H = matrix( [-0.05, 0.05, -0.1, 0.1, -100, 100, -pi, pi, -pi, pi, -pi, pi]) # Define Task Space Regions # Left Hand TSRString1 = SerializeTSR(0, 'NULL', T0_w0L, Tw0_eL, Bw0L) # Right Hand TSRString2 = SerializeTSR(1, 'crank crank', T0_w0R, Tw0_eR, Bw0R) # Left Foot TSRString3 = SerializeTSR(2, 'NULL', Tee[2], eye(4), matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])) # Right Foot TSRString4 = SerializeTSR(3, 'NULL', Tee[3], eye(4), matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0])) # Head TSRString5 = SerializeTSR(4, 'NULL', T0_w0H, Tw0_eH, Bw0H) TSRChainStringFeetandHead = SerializeTSRChain( 0, 0, 1, 1, TSRString3, 'NULL', []) + ' ' + SerializeTSRChain( 0, 0, 1, 1, TSRString4, 'NULL', []) + ' ' + SerializeTSRChain( 0, 0, 1, 1, TSRString5, 'NULL', []) TSRChainString = SerializeTSRChain( 0, 0, 1, 1, TSRString1, 'crank', matrix( [crankjointind])) + ' ' + SerializeTSRChain( 0, 0, 1, 1, TSRString2, 'NULL', matrix( [])) + ' ' + TSRChainStringFeetandHead # Calculate hand transforms after rotating the wheel (they will help us find the goalik): # How much do we want to rotate the wheel? crank_rot = pi / 6.5 # Which joint do we want the CBiRRT to mimic the TSR for? TSRChainMimicDOF = 1 # Create the transform for the wheel that we would like to reach to Tcrank_rot = MakeTransform(rodrigues([crank_rot, 0, 0]), transpose(matrix([0, 0, 0]))) # What is this? temp = MakeTransform(rodrigues([0, 0, crank_rot]), transpose(matrix([0, 0, 0]))) # Rotate the left hand's transform on the wheel in world transform "crank_rot" radians around it's Z-Axis T0_cranknew = dot(T0_w0L, Tcrank_rot) # Where will the left hand go after turning the wheel? # This is what's happening: # # Tcranknew_LH2 = dot(Tw0L_0,T0_LH1) --> Left hand in wheel's coordinate # T0_LH2 = dot(T0_cranknew,Tcranknew_LH2) --> Left hand rotated around wheel's origin T0_LH2 = dot(T0_cranknew, dot(linalg.inv(T0_w0L), T0_LH1)) # Uncomment to see T0_LH2 handles.append(misc.DrawAxes(self.env, matrix(T0_LH2), 1)) # Where will the right hand go after turning the wheel? T0_RH2 = dot(T0_crankcrank, dot(temp, dot(linalg.inv(T0_crankcrank), T0_RH1))) # Uncomment to see T0_RH2 handles.append(misc.DrawAxes(self.env, matrix(T0_RH2), 1)) if (self.StopAtKeyStrokes): print "Press Enter to find a goalIK" sys.stdin.readline() self.crankid.SetDOFValues([crank_rot], [crankjointind]) arg1 = str(cogtarg).strip("[]").replace(', ', ' ') arg2 = trans_to_str(T0_LH2) arg3 = trans_to_str(T0_RH2) goalik = probs[0].SendCommand('DoGeneralIK exec supportlinks 2 ' + footlinknames + ' movecog ' + arg1 + ' nummanips 2 maniptm 0 ' + arg2 + ' maniptm 1 ' + arg3) self.robotid.SetActiveDOFValues(str2num(goalik)) self.crankid.SetDOFValues([crank_rot], [crankjointind]) self.crankid.SetDOFValues([0], [crankjointind]) self.robotid.SetActiveDOFValues(initconfig) self.robotid.SetDOFValues(self.rhandopenvals, self.rhanddofs) self.robotid.SetDOFValues(self.lhandopenvals, self.lhanddofs) if (self.StopAtKeyStrokes): print "Press Enter to plan initconfig --> startik" sys.stdin.readline() # Get a trajectory from initial configuration to grasp configuration with self.robotid: try: answer = probs[0].SendCommand( 'RunCBiRRT psample 0.2 supportlinks 2 ' + footlinknames + ' smoothingitrs ' + str(normalsmoothingitrs) + ' ' + TSRChainStringGrasping) print "RunCBiRRT answer: ", str(answer) except openrave_exception, e: print "Cannot send command RunCBiRRT: " print e
print arg1 print "arg2 : " print arg2 print "arg3 : " print arg3 sys.stdin.readline() startik = probs[0].SendCommand('DoGeneralIK exec supportlinks 2 ' + footlinknames + ' movecog ' +arg1+ ' nummanips 2 maniptm 1 ' + arg2 + ' maniptm 2 ' + arg3 ); #startik = probs[0].SendCommand('DoGeneralIK exec nummanips 1 maniptm 4 '+arg2) print "Got startik:" print startik print "------" robotid.SetDOFValues(str2num(startik)) # Note: startik: T0_LH1 and T0_RH1 print "go to startik" sys.stdin.readline() T0_RH2 = T0_RH1 #T0_RH2 = Tee[1] T0_RH2[0,3] -= 0.1 T0_RH2[1,3] -= 0.2 Rotate_RH2 = MakeTransform(matrix(xyz_rotation([0,-pi/4,-pi/3])),transpose(matrix([0,0,0]))) T0_RH2 = dot(T0_RH2,Rotate_RH2) h2 = misc.DrawAxes(env,matrix(T0_RH2),1) #T0_RH2[2,3] += 0.01 #arg1 = str(cogtarg).strip("[]")
self.robotid.WaitForController(0) self.robotid.GetController().Reset(0) self.robotid.SetDOFValues(self.rhandclosevals,self.rhanddofs) self.robotid.SetDOFValues(self.lhandclosevals,self.lhanddofs) if( self.StopAtKeyStrokes ): print "Press Enter to plan startik --> goalik " sys.stdin.readline() # Get a trajectory from goalik to grasp configuration goaljoints = deepcopy(goalik) for i in range(TSRChainMimicDOF): goaljoints += ' 0' goaljoints = str2num(goaljoints) try: answer = probs[0].SendCommand('RunCBiRRT supportlinks 2 '+footlinknames+' smoothingitrs '+str(fastsmoothingitrs)+' jointgoals '+str(len(goaljoints))+' '+Serialize1DMatrix(matrix(goaljoints))+' '+TSRChainString) print "RunCBiRRT answer: ",str(answer) except openrave_exception, e: print "Cannot send command RunCBiRRT: " print e try: os.rename("cmovetraj.txt","movetraj1.txt") except OSError, e: # No file cmovetraj print e try:
# Went through all candidates print "Found ",str(len(collisionFreeSolutions[0]))," valid solutions in ",str(howMany)," candidates." if (len(collisionFreeSolutions[0]) > 0) : # Have we found at least 1 valid path? success = True print "Press Enter to iterate through valid solutions." sys.stdin.readline() nxt = 0 ask = True playAll = False while(True): print "Playing valid solution #: ",str(nxt) robots[0].SetActiveDOFValues(str2num(collisionFreeSolutions[1][nxt])) play(T0_starts, whereToFace, relBaseConstraint,candidates,numRobots,numManips,collisionFreeSolutions[0][nxt],myRmaps,robots,h,env,0.3,False,' leftFootBase rightFootBase ') myCOM = array(get_robot_com(robots[0])) myCOM[2,3] = 0.0 COMHandle = misc.DrawAxes(env,myCOM,0.3) h.pop() # delete the robot base axis we added last # Finally plan a trajectory from startik to goalik startikStr = collisionFreeSolutions[1][nxt] wheel.SetDOFValues([0],[0]) go_to_startik(robots[0], startikStr) temp1 = MakeTransform(rodrigues([-pi/2,0,0]),transpose(matrix([0,0,0]))) temp2 = MakeTransform(rodrigues([0,0,-pi/2]),transpose(matrix([0,0,0]))) CTee = wheel.GetManipulators()[0].GetEndEffectorTransform()
def start(T0_starts, T0_FACING, candidates, numRobots, numManips, c, myRmaps, robots, h, myEnv, doGeneralIk, footlinknames=''): # Define this variable so python doesn't complain myIK = '' # constraints to check masterBaseConstOK = True # Move each robot / manipulators for myRobotIndex in range(numRobots): for myManipulatorIndex in range(numManips[myRobotIndex]): # Find where to move the base startSphereIndex = candidates[myManipulatorIndex][c][0].sIdx startTransformIndex = candidates[myManipulatorIndex][c][0].tIdx # Set the manipulator to its configuration (we will check for collision) # print "for manipulator: ",str(myManipulatorIndex) # print "go_to: start_sphere_index: " # print startSphereIndex # print "go_to: start_transform_index: " # print startTransformIndex myRmaps[myManipulatorIndex].go_to(startSphereIndex, startTransformIndex) Tbase_start = myRmaps[myManipulatorIndex].map[startSphereIndex].T[ startTransformIndex] # for myT in myRmaps[myManipulatorIndex].map[startSphereIndex].T: # print myT T0_newManipPose = dot(T0_starts[myManipulatorIndex], linalg.inv(Tbase_start)) # Finally move the robot base robots[myRobotIndex].SetTransform(T0_newManipPose) # print "manip index: ",str(myManipulatorIndex) # sys.stdin.readline() if (myManipulatorIndex == 0): h.append(misc.DrawAxes(myEnv, T0_newManipPose, 0.4)) # Check master base constraint # if(myRobotIndex == 0): # if(type(masterBaseConstraint) == type([])): # if((abs(T0_newManipPose[0:3,3].transpose()) > masterBaseConstraint[0:3]).any()): # masterBaseConstOK = False # break # #if(not allclose(T0_newManipPose[0:3,0:3],rodrigues(masterBaseConstraint[3:6]))): # # masterBaseConstOK = False # # HACK-AROUND # # For now just check if the robot base is on XY plane # if((not allclose(T0_newManipPose[0:3,2].transpose(),[0,0,1])) or (not allclose(T0_newManipPose[2,0:3],[0,0,1]))): # masterBaseConstOK = False # break # elif(type(masterBaseConstraint) == type(array(()))): # pass # Now check if the feet are on the ground # # TODO: We should do this in a robot-agnostic way. # masterBaseConstraint works for mobile manipulators # or for industrial arms, but for humanoids the base # and the feet can be different, so for balance check # we "may" need an extra step. How do we set this flag? # Maybe we should have a "isHumanoid" bool? And try to # put the feet on the ground if(myReachabilityMap.isHumanoid) # if (doGeneralIk): # Reset the head and the legs to zero robots[myRobotIndex].SetDOFValues([0.0, 0.0, 0.0, 0.0], [6, 7, 8, 9]) robots[myRobotIndex].SetDOFValues([0.0, 0.0, 0.0], [24, 25, 29]) robots[myRobotIndex].SetDOFValues([0.0, 0.0, 0.0], [30, 31, 35]) # Bend the knees to avoid singularity issues when solving GeneralIK robots[myRobotIndex].SetDOFValues([-0.3, 0.6, -0.3], [32, 33, 34]) robots[myRobotIndex].SetDOFValues([-0.3, 0.6, -0.3], [26, 27, 28]) # print "trying to put the feet on the ground..." if (footlinknames == ''): myIK = put_feet_on_the_ground(robots[myRobotIndex], T0_FACING, myEnv) else: myIK = put_feet_on_the_ground(robots[myRobotIndex], T0_FACING, myEnv, footlinknames) if (myIK != ''): # if found a solution show it. robots[myRobotIndex].SetActiveDOFValues(str2num(myIK)) else: masterBaseConstOK = False # Check collision with self and with the environment if (myEnv.CheckCollision(robots[myRobotIndex]) or robots[myRobotIndex].CheckSelfCollision()): collisionConstOK = False else: collisionConstOK = True return [(masterBaseConstOK and collisionConstOK), myIK]
# print arg4 print "Press Enter to find a goalIK" sys.stdin.readline() crankid.SetDOFValues([crank_rot], [crankjointind]) goalik = probs[0].SendCommand('DoGeneralIK exec supportlinks 2 ' + footlinknames + ' movecog ' + arg1 + ' nummanips 3 maniptm 0 ' + arg2 + ' maniptm 1 ' + arg3 + ' maniptm 2 ' + arg4) # print "goalIK" # print goalik robotid.SetActiveDOFValues(str2num(goalik)) crankid.SetDOFValues([crank_rot], [crankjointind]) print "Press Enter to go to startik" sys.stdin.readline() # Get a trajectory from goalik to grasp configuration goaljoints = deepcopy(goalik) for i in range(TSRChainMimicDOF): goaljoints += ' 0' goaljoints = str2num(goaljoints) robotid.SetActiveDOFValues(startik) # Close hands to start "turning" the wheel robotid.SetDOFValues(rhandclosevals, rhanddofs)
def test_points(self): print "Testing points" self.notPlanning = False # We are planning: this will stop robot model and wheel model being updated manips = self.robot.GetManipulators() crankmanip = self.crankid.GetManipulators() print "Getting Loaded Problems" self.probs = self.env.GetLoadedProblems() print self.probs[0] # --> self.probs_cbirrt print self.probs[1] # --> self.probs_crankmover self.robot.SetActiveDOFs([15,16,17,18,19,20,21,27,28,29,30,31,32,33]) # InSyncWithROS sets DOF values for both OpenRAVE and Gazebo self.SetRobotDOFValuesInSyncWithROS(self.robot,[-0.85,0.3,-1.00,-1.57,0.0,-1.00,-1.00,0.05],[27,28,29,30,31,32,33,34]) self.SetRobotDOFValuesInSyncWithROS(self.robot,[0.85, 0.3,1.00,-1.57,0.0,-1.00,1.00,0.05],[15,16,17,18,19,20,21,22]) self.initconfig = self.robot.GetActiveDOFValues() print "initconfig" print self.initconfig print type(self.initconfig) manip = self.robot.SetActiveManipulator('leftarm') # set the manipulator to leftarm self.T0_LH_INIT = manip.GetEndEffectorTransform() # end of manipulator 7 manip = self.robot.SetActiveManipulator('rightarm') # set the manipulator to leftarm self.T0_RH_INIT = manip.GetEndEffectorTransform() # end of manipulator 5 self.robot.SetActiveDOFs([15,16,17,18,19,20,21,27,28,29,30,31,32,33]) print "Press enter to continue 1..." sys.stdin.readline() links = self.robot.GetLinks() self.crankjointind = 0 self.jointtm = self.probs[0].SendCommand('GetJointTransform name crank jointind '+str(self.crankjointind)) maniptm = self.crankid.GetManipulators()[0].GetTransform() self.jointtm = self.jointtm.replace(" ",",") self.jointtm = eval('['+self.jointtm+']') rhanddofs = [34] # Right gripper links here rhandclosevals = [0.035] # What are the closed joint values for the right hand's links?--> Obtained experimentally rhandopenvals = [0.0548] # What are the open joint values for the right hand's links? --> Obtained experimentally lhanddofs = [22] # Left gripper links here lhandclosevals = [0.035] # Same as rhandclosevals for the left gripper --> Obtained experimentally lhandopenvals = [0.0548] # Same as rhandopenvals for the left gripper --> Obtained experimentally ############################################################## ## THIS IS WHERE THE FIRST BLOCK ENDS IN THE MATLAB VERSION ## ############################################################## self.robot.SetActiveDOFs([15,16,17,18,19,20,21,27,28,29,30,31,32,33]) # Rotate the driving wheel's manipulator's coordinate frame 90 degrees around its Z-axis temp = dot(maniptm,MakeTransform(matrix(rodrigues([0,0,pi/2])),transpose(matrix([0,0,0])))) # Rotate the new coordinate frame -90 degrees around its X-axis temp = dot(temp,MakeTransform(matrix(rodrigues([-pi/2,0,0])),transpose(matrix([0,0,0])))) # Rotate the new coordinate frame -30 degrees around its Y-axis temp = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0,0,0])))) # Translate the new coordinate frame -0.11 meters on its Z-axis and assign it to Left Hand self.T0_LH1_APPROACH_1 = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.1,0.0,-0.2])))) self.T0_LH1_APPROACH_2 = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0,0.0,-0.18])))) self.T0_LH1_ENTRY = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0,0.0,-0.155])))) # Rotate the driving wheel's manipulator's coordinate frame 90 degrees around its Z-axis temp = dot(maniptm,MakeTransform(matrix(rodrigues([0,0,pi/2])),transpose(matrix([0,0,0])))) # Rotate the new coordinate frame +90 degrees around its X-axis temp = dot(temp,MakeTransform(matrix(rodrigues([pi/2,0,0])),transpose(matrix([0,0,0])))) # Rotate the new coordinate frame 30 degrees around its Y-axis temp = dot(temp,MakeTransform(matrix(rodrigues([0,pi/6,0])),transpose(matrix([0,0,0])))) # Translate the new coordinate frame -0.11 meters on its Z-axis and assign it to Left Hand self.T0_RH1_APPROACH_1 = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.1,0.0,-0.2])))) self.T0_RH1_APPROACH_2 = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0,0.0,-0.18])))) self.T0_RH1_ENTRY = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0,0.0,-0.155])))) # debug: at this point, it can be useful to visualize where we want our hand to move (use visualization_msgs / markers?) arg1 = str(GetRot(self.T0_LH1_APPROACH_1)).strip("[]")+str(GetTrans(self.T0_LH1_APPROACH_1)).strip("[]") arg1 = arg1.replace("\n"," ") arg2 = str(GetRot(self.T0_RH1_APPROACH_1)).strip("[]")+str(GetTrans(self.T0_RH1_APPROACH_1)).strip("[]") arg2 = arg2.replace("\n"," ") self.approachik_1 = self.probs[0].SendCommand('DoGeneralIK exec nummanips 2 maniptm 7 '+arg1+' maniptm 5 '+arg2) print "Got self.approachik_1:" if self.approachik_1 == '': print "Hey: No IK Solution found! Move the robot!" self.notPlanning = True return [] else: print str2num(self.approachik_1) print type(str2num(self.approachik_1)) print "------" self.robot.SetActiveDOFValues(str2num(self.approachik_1)) self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.approachik_1),self.robot.GetActiveDOFIndices()) print "went to approach-1" sys.stdin.readline() ################################################################ ## THIS IS WHERE THE SECOND BLOCK ENDS IN THE MATLAB VERSION ## ## APPROACH-1 IK IS OVER - NOW GO TO APPROACH-2 ## ################################################################ arg1 = str(GetRot(self.T0_LH1_APPROACH_2)).strip("[]")+str(GetTrans(self.T0_LH1_APPROACH_2)).strip("[]") arg1 = arg1.replace("\n"," ") arg2 = str(GetRot(self.T0_RH1_APPROACH_2)).strip("[]")+str(GetTrans(self.T0_RH1_APPROACH_2)).strip("[]") arg2 = arg2.replace("\n"," ") self.approachik_2 = self.probs[0].SendCommand('DoGeneralIK exec nummanips 2 maniptm 7 '+arg1+' maniptm 5 '+arg2) print "Got approachik-2:" if self.approachik_2 == '': print "Hey: No IK Solution found! Move the robot!" self.notPlanning = True return [] else: print self.approachik_2 print "------" self.robot.SetActiveDOFValues(str2num(self.approachik_2)) self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.approachik_2),self.robot.GetActiveDOFIndices()) print "went to approach-2" sys.stdin.readline() #################################################################### ## APPROACH-2 IK IS OVER - NOW GO TO STARTIK WHERE WE WILL GRASP ## #################################################################### arg1 = str(GetRot(self.T0_LH1_ENTRY)).strip("[]")+str(GetTrans(self.T0_LH1_ENTRY)).strip("[]") arg1 = arg1.replace("\n"," ") arg2 = str(GetRot(self.T0_RH1_ENTRY)).strip("[]")+str(GetTrans(self.T0_RH1_ENTRY)).strip("[]") arg2 = arg2.replace("\n"," ") self.startik = self.probs[0].SendCommand('DoGeneralIK exec nummanips 2 maniptm 7 '+arg1+' maniptm 5 '+arg2) print "Got self.startik:" if self.startik == '': print "Hey: No IK Solution found! Move the robot!" self.notPlanning = True return [] else: print self.startik print "------" self.robot.SetActiveDOFValues(str2num(self.startik)) self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.startik),self.robot.GetActiveDOFIndices()) print "went to startik" sys.stdin.readline() ################################################### ## COLLISION FREE REACHING TO GRASP WITH CBiRRT ## ################################################### ## Now let's try a different version of the previous block. ## The previous block tries to go to initial configuration without any collision avoidance. ## IF we want to avoid colliding with the wheel, we need to define Task Space Regions: # TSRString1 = SerializeTSR(5,'NULL',T0_RH1,eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0])) # RightHand_Torso manip: 4 # TSRString2 = SerializeTSR(7,'NULL',T0_LH1,eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0])) # LeftHand_Torso manip: 6 # #TSRChainStringGoToStartIK = SerializeTSRChain(0,1,0,1,TSRString1,'NULL',[])+' '+SerializeTSRChain(0,1,0,1,TSRString2,'NULL',[]) # TSRChainStringGoToStartIK = SerializeTSRChain(0,1,0,1,TSRString2,'NULL',[]) # #answer = probs[0].SendCommand('RunCBiRRT psample 0.2 smoothingitrs 150 %s'%(TSRChainStringGoToStartIK)) # answer = probs[0].SendCommand('RunCBiRRT psample 0.25 %s'%(TSRChainStringGoToStartIK)) # # Defined the TSRs as constraints during the trajectory execution. Now let's send them over to the planner. # print answer # if (answer == 1): # print "runcbirrt successful for finding a path to start ik without collision with the wheel" # startik = self.robot.GetDOFValues() # print "Got startik:" # print startik # print "------" # # Rename the file so that we can keep the data w/o overwriting it with a new trajectory # try: # os.rename("cmovetraj.txt","movetraj-goto-startik.txt") # if os.path.isfile('movetraj-goto-startik.txt'): # try: # answer = pr2_rave2task_control('movetraj-goto-startikt.txt') # except openrave_exception, e: # print e # self.robot.WaitForController(0) # print "Press enter to restart" # sys.stdin.readline() # else: # print "Can't find movetraj-goto-startik.txt" # except OSError, e: # print e # print "went to startik" # sys.stdin.readline() # else: # print "Hey: No IK Solution found! Move the robot!" # print T0_LH1 # print T0_RH1 # self.notPlanning = True # return [] ############################################################## ## END OF STARTIK TSR DEFINITION AND TRAJECTORY EXEC. BLOCK ## ############################################################## # Left hand is evaluated first, so need to make right hand relative to crank cranklinks = self.crankid.GetLinks(); self.T0_crankcrank = self.crankid.GetManipulators()[0].GetTransform() T0_w0L = eye(4) T0_w0R = MakeTransform(rodrigues([0,self.wristPitch,0]),matrix([[self.jointtm[9]],[self.jointtm[10]],[self.jointtm[11]]])) crank_rot = pi/6 TSRChainMimicDOF = 1 Tcrank_rot = MakeTransform(matrix(rodrigues([crank_rot,0,0])),transpose(matrix([0,0,0]))) # For the right gripper Tcrank_rot2 = MakeTransform(matrix(rodrigues([0,0,crank_rot])),transpose(matrix([0,0,0]))); # For the left gripper T0_cranknew = dot(T0_w0R,Tcrank_rot) temp = dot(linalg.inv(T0_w0R),self.T0_RH1_ENTRY) T0_RH2 = dot(T0_cranknew,temp); temp = dot(linalg.inv(self.T0_crankcrank),self.T0_LH1_ENTRY) temp = dot(Tcrank_rot2,temp) T0_LH2 = dot(self.T0_crankcrank,temp) arg1 = str(GetRot(T0_LH2)).strip("[]")+str(GetTrans(T0_LH2)).strip("[]") arg2 = str(GetRot(T0_RH2)).strip("[]")+str(GetTrans(T0_RH2)).strip("[]") self.crankid.SetDOFValues([crank_rot],[self.crankjointind]) # Get the goal inverse kinematics self.goalik = self.probs[0].SendCommand('DoGeneralIK exec nummanips 2 maniptm 7 '+arg1+' maniptm 5 '+arg2) print "Got self.goalik:" if self.startik == '': print "Hey: No IK Solution found! Change the target!" self.notPlanning = True return [] else: print self.goalik print "------" #self.robot.SetActiveDOFValues(str2num(goalik)) # Note: self.goalik is T0_LH2 and T0_RH2 self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.goalik),self.robot.GetActiveDOFIndices()) print "went to goalik" sys.stdin.readline() # Let the TSR Magic Happen self.crankid.SetDOFValues([crank_rot],[self.crankjointind]) ######################################################### # This is the end of the second block in Matlab version # ######################################################### self.crankid.SetDOFValues([self.crankjointind]) #self.robot.SetActiveDOFValues(str2num(startik)) self.SetRobotDOFValuesInSyncWithROS(self.robot,str2num(self.startik),self.robot.GetActiveDOFIndices()) ################################################ ## YOU'RE AT STARTIK - GO BACK TO APPROACH 2 ## ################################################ goaljoints = str2num(self.approachik_2); print "Press enter to go to approach point 2" sys.stdin.readline() self.SetRobotDOFValuesInSyncWithROS(self.robot,goaljoints,self.robot.GetActiveDOFIndices()) ################################################## ## YOU'RE AT APPROACH 2 - GO BACK TO APPROACH 1 ## ################################################## goaljoints = str2num(self.approachik_1); print "Press enter to go to approach point 1" sys.stdin.readline() self.SetRobotDOFValuesInSyncWithROS(self.robot,goaljoints,self.robot.GetActiveDOFIndices()) ###################################################### ## YOU'RE AT APPROACH 1 - GO BACK TO INITCONFIG ## ###################################################### print "Press enter to go to initconfig" sys.stdin.readline() self.SetRobotDOFValuesInSyncWithROS(self.robot,self.initconfig,self.robot.GetActiveDOFIndices()) return 1
def start(T0_starts, T0_FACING, candidates,numRobots,numManips,c,myRmaps,robots,h,myEnv,doGeneralIk,footlinknames=''): # Define this variable so python doesn't complain myIK = '' # constraints to check masterBaseConstOK = True # Move each robot / manipulators for myRobotIndex in range(numRobots): for myManipulatorIndex in range(numManips[myRobotIndex]): # Find where to move the base startSphereIndex = candidates[myManipulatorIndex][c][0].sIdx startTransformIndex = candidates[myManipulatorIndex][c][0].tIdx # Set the manipulator to its configuration (we will check for collision) # print "for manipulator: ",str(myManipulatorIndex) # print "go_to: start_sphere_index: " # print startSphereIndex # print "go_to: start_transform_index: " # print startTransformIndex myRmaps[myManipulatorIndex].go_to(startSphereIndex,startTransformIndex) Tbase_start = myRmaps[myManipulatorIndex].map[startSphereIndex].T[startTransformIndex] # for myT in myRmaps[myManipulatorIndex].map[startSphereIndex].T: # print myT T0_newManipPose = dot(T0_starts[myManipulatorIndex],linalg.inv(Tbase_start)) # Finally move the robot base robots[myRobotIndex].SetTransform(T0_newManipPose) # print "manip index: ",str(myManipulatorIndex) # sys.stdin.readline() if(myManipulatorIndex == 0): h.append(misc.DrawAxes(myEnv,T0_newManipPose,0.4)) # Check master base constraint # if(myRobotIndex == 0): # if(type(masterBaseConstraint) == type([])): # if((abs(T0_newManipPose[0:3,3].transpose()) > masterBaseConstraint[0:3]).any()): # masterBaseConstOK = False # break # #if(not allclose(T0_newManipPose[0:3,0:3],rodrigues(masterBaseConstraint[3:6]))): # # masterBaseConstOK = False # # HACK-AROUND # # For now just check if the robot base is on XY plane # if((not allclose(T0_newManipPose[0:3,2].transpose(),[0,0,1])) or (not allclose(T0_newManipPose[2,0:3],[0,0,1]))): # masterBaseConstOK = False # break # elif(type(masterBaseConstraint) == type(array(()))): # pass # Now check if the feet are on the ground # # TODO: We should do this in a robot-agnostic way. # masterBaseConstraint works for mobile manipulators # or for industrial arms, but for humanoids the base # and the feet can be different, so for balance check # we "may" need an extra step. How do we set this flag? # Maybe we should have a "isHumanoid" bool? And try to # put the feet on the ground if(myReachabilityMap.isHumanoid) # if(doGeneralIk): # Reset the head and the legs to zero robots[myRobotIndex].SetDOFValues([0.0, 0.0, 0.0, 0.0],[6,7,8,9]) robots[myRobotIndex].SetDOFValues([0.0, 0.0, 0.0],[24,25,29]) robots[myRobotIndex].SetDOFValues([0.0, 0.0, 0.0],[30,31,35]) # Bend the knees to avoid singularity issues when solving GeneralIK robots[myRobotIndex].SetDOFValues([-0.3,0.6,-0.3],[32,33,34]) robots[myRobotIndex].SetDOFValues([-0.3,0.6,-0.3],[26,27,28]) # print "trying to put the feet on the ground..." if(footlinknames==''): myIK = put_feet_on_the_ground(robots[myRobotIndex], T0_FACING, myEnv) else: myIK = put_feet_on_the_ground(robots[myRobotIndex], T0_FACING, myEnv, footlinknames) if(myIK != ''): # if found a solution show it. robots[myRobotIndex].SetActiveDOFValues(str2num(myIK)) else: masterBaseConstOK = False # Check collision with self and with the environment if(myEnv.CheckCollision(robots[myRobotIndex]) or robots[myRobotIndex].CheckSelfCollision()): collisionConstOK = False else: collisionConstOK = True return [(masterBaseConstOK and collisionConstOK), myIK]
handtrans1 = T0_object[0:3, 3] + transoffset1 handrot1 = mat(rodrigues([pi / 2, 0, 0])) Tik0 = MakeTransform(handrot0, handtrans0) Tik1 = MakeTransform(handrot1, handtrans1) #get the ik solutions for both arms for the box in the start pose robot.SetActiveDOFs(arm0dofs) startik0 = probs_cbirrt.SendCommand( 'DoGeneralIK exec nummanips 1 maniptm 0 %s' % SerializeTransform(Tik0)) robot.SetActiveDOFs(arm1dofs) startik1 = probs_cbirrt.SendCommand( 'DoGeneralIK exec nummanips 1 maniptm 1 %s' % SerializeTransform(Tik1)) startik = '%s %s' % (startik0, startik1) robot.SetActiveDOFs(activedofs) robot.SetActiveDOFValues(str2num(startik)) #define the target transform of the goal T0_object2 = MakeTransform(mat(eye(3)), mat([0.6923, 0.00, 1.3989]).T) targobject.SetTransform(array(T0_object2[0:3][:, 0:4])) #define targets for both hands relative to box in goal pose handtrans0 = T0_object2[0:3, 3] + transoffset0 handtrans1 = T0_object2[0:3, 3] + transoffset1 Tik0 = MakeTransform(handrot0, handtrans0) Tik1 = MakeTransform(handrot1, handtrans1) #get the ik solutions for both arms for the box in the goal pose robot.SetActiveDOFs(arm0dofs) goalik0 = probs_cbirrt.SendCommand( 'DoGeneralIK exec nummanips 1 maniptm 0 %s' % SerializeTransform(Tik0))
def play(T0_starts, T0_FACING, relBaseConstraint,candidates,numRobots,numManips,c,myRmaps,robots,h,myEnv,howLong,doGeneralIk,footlinknames=''): # constraints to check relBaseConstOK = True collisionConstOK = True noConfigJump = True # Get the length of the candidate path # First index stands for the robot index, and the second index stands for the candidate path index. We're calling find_random_candidates() function with an argument of 1. Thus there will be only one candidate returned. Let's find it's length. pathLength = min(len(candidates[0][c]), len(candidates[1][c])) # print "numRobots: ",str(numRobots) # print "numManips: ",str(numManips) [goAhead, activeDOFStartConfig] = start(T0_starts,T0_FACING, candidates,numRobots,numManips,c,myRmaps,robots,h,myEnv,doGeneralIk,footlinknames) if(goAhead): # Get their relative Transformation matrix T0_base = [] for myRobotIndex in range(numRobots): for myManipulatorIndex in range(numManips[myRobotIndex]): T0_base.append(robots[myRobotIndex].GetManipulators()[myManipulatorIndex].GetBase().GetTransform()) Trobot0_robot1 = dot(linalg.inv(T0_base[0]),T0_base[1]) # i) Does the candidate satisfy the robot base transform constraint? if(type(relBaseConstraint) == type([])): # if the input argument type is a list then we have bounds # Check translation constraints if((abs(Trobot0_robot1[0:3,3].transpose()) > relBaseConstraint[0:3]).any()): relBaseConstOK = False return [False, ''] # Check rotation constraints if(not allclose(Trobot0_robot1[0:3,0:3],rodrigues(relBaseConstraint[3:6]))): relBaseConstOK = False return [False, ''] elif(type(relBaseConstraint) == type(array(()) or relBaseConstraint) == type(matrix(()))): # if input argument type is a numpy array then we have an exact transformation #print "relBaseConstraint?" #print allclose(Trobot0_robot1,relBaseConstraint) if(not allclose(Trobot0_robot1,relBaseConstraint)): relBaseConstOK = False return [False, ''] # If the solution meets the base constraint: if(relBaseConstOK): # print "Base Constraints OK." pathConfigs = [[],[]] # A 2D List # ii) Check if the solution collision-free throughout the path? # For each path element, go step by step and check prevConfig = [[],[]] currentConfig = [[],[]] # print "----" for pElementIndex in range(pathLength): # Move the manipulator to this path element for myRobotIndex in range(numRobots): for myManipulatorIndex in range(numManips[myRobotIndex]): currentSphereIndex = candidates[myManipulatorIndex][c][pElementIndex].sIdx currentTransformIndex = candidates[myManipulatorIndex][c][pElementIndex].tIdx myRmaps[myManipulatorIndex].go_to(currentSphereIndex,currentTransformIndex) pathConfigs[myManipulatorIndex].append(robots[myRobotIndex].GetDOFValues(robots[myRobotIndex].GetManipulators()[myManipulatorIndex].GetArmIndices())) # DEBUG SECTION FOR SENSING CONFIGURATION JUMP currentConfig[myManipulatorIndex] = robots[myRobotIndex].GetDOFValues(robots[myRobotIndex].GetManipulators()[myManipulatorIndex].GetArmIndices()) # print "For robot ",str(myRobotIndex)," ||qA-qB||:" # print "path element ",str(pElementIndex) if(prevConfig[myManipulatorIndex] != []): # print "previous config: " # print prevConfig[myRobotIndex] # print "current config: " # print currentConfig[myRobotIndex] qdiff = absolute(subtract(currentConfig[myManipulatorIndex],prevConfig[myManipulatorIndex])) configDistSq = 0 # for each joint do: for j in range(len(qdiff)): configDistSq += pow(qdiff[j],2) # find ||qA-qB|| euclideanConfigDistance = pow(configDistSq,0.5) # print "euclidean configuration distance: " # print euclideanConfigDistance # if(euclideanConfigDistance > configurationJumpThreshold): # noConfigJump = False # return False else: #print "skipping path element:" pass # END OF DEBUG SECTION FOR SENSING CONFIGURATION JUMP prevConfig[myManipulatorIndex] = deepcopy(currentConfig[myManipulatorIndex]) # Check collision with self and with the environment if(myEnv.CheckCollision(robots[myRobotIndex]) or robots[myRobotIndex].CheckSelfCollision()): collisionConstOK = False return [False, ''] # After setting manipulator configurations for this # robot, check if the center of mass is close enough # to the center of robot's feet # # TODO: This part is very messy and ugly. # I need to clean it up and make nice function calls. if(doGeneralIk): if(footlinknames==''): myIK = put_feet_on_the_ground(robots[myRobotIndex], T0_FACING, myEnv) else: myIK = put_feet_on_the_ground(robots[myRobotIndex], T0_FACING, myEnv, footlinknames) if(myIK != ''): # if successful, show it robots[myRobotIndex].SetActiveDOFValues(str2num(myIK)) # print "checking support in play..." if(not check_support(array(get_robot_com(robots[myRobotIndex])),robots[myRobotIndex])): return [False, ''] else: pass # print "in balance - path element: ",str(pElementIndex) #sys.stdin.readline() else: return [False, ''] # If you didn't break yet, wait before the next path element for visualization time.sleep(howLong) for manipIdx, manipConfs in enumerate(pathConfigs): # print "for manip ",str(manipIdx) for confIdx, conf in enumerate(manipConfs): if(confIdx > 0): qdiff = absolute(subtract(conf,manipConfs[confIdx-1])) confDistSq = 0 for j in range(len(qdiff)): configDistSq += pow(qdiff[j],2) eConfDist = pow(configDistSq,0.5) # print "euclidean configuration distance:" # print eConfDist if(eConfDist > configurationJumpThreshold): noConfigJump = False return [False, ''] # If you made it here, # it means no configuration jump, no collision, and # the center of mass was close enough to the center # of robot's feet for all reachability spheres. return [True, activeDOFStartConfig] else: # start() failed return [False, '']
def go_to_startik(myRobot, startik): # print "Went to startik" myRobot.SetActiveDOFValues(str2num(startik)) time.sleep(0.05) if(debug): sys.stdin.readline()
myCBiRRTProblem = RaveCreateModule(myEnv,'CBiRRT') myManipulationProblem = RaveCreateModule(myEnv,'Manipulation') try: myEnv.AddModule(myCBiRRTProblem,myRobot.GetName()) # this string should match to <Robot name="" > in robot.xml myEnv.AddModule(myManipulationProblem,myRobot.GetName()) # this string should match to <Robot name="" > in robot.xml except openrave_exception, e: print e #grab the object with manipulator 0 myManipulationProblem.SendCommand('setactivemanip index 0') myManipulationProblem.SendCommand('GrabBody name '+kinBodyToGrab) jointgoalsStr = deepcopy(goalikStr) jointgoalsNum = str2num(jointgoalsStr) try: # answer = myCBiRRTProblem.SendCommand('RunCBiRRT timelimit 5 smoothingitrs 1 jointgoals %s %s %s'%(len(jointgoalsNum),Serialize1DMatrix(mat(jointgoalsNum)),TSRChainString)) answer = myCBiRRTProblem.SendCommand('RunCBiRRT timelimit 5 supportlinks 2 '+footlinknames+' smoothingitrs '+str(fastsmoothingitrs)+' jointgoals '+str(len(jointgoalsNum))+' '+Serialize1DMatrix(matrix(jointgoalsNum))+' '+TSRChainString) print print "RunCBiRRT answer: ",str(answer) except openrave_exception, e: print "Cannot send command RunCBiRRT: " print e # cleanup the cbirrt problem object myEnv.Remove(myCBiRRTProblem) myEnv.Remove(myManipulationProblem)
print "arg3 : " print arg3 sys.stdin.readline() startik = probs[0].SendCommand('DoGeneralIK exec supportlinks 2 ' + footlinknames + ' movecog ' + arg1 + ' nummanips 2 maniptm 1 ' + arg2 + ' maniptm 2 ' + arg3) #startik = probs[0].SendCommand('DoGeneralIK exec nummanips 1 maniptm 4 '+arg2) print "Got startik:" print startik print "------" robotid.SetDOFValues(str2num(startik)) # Note: startik: T0_LH1 and T0_RH1 print "go to startik" sys.stdin.readline() T0_RH2 = T0_RH1 #T0_RH2 = Tee[1] T0_RH2[0, 3] -= 0.1 T0_RH2[1, 3] -= 0.2 Rotate_RH2 = MakeTransform(matrix(xyz_rotation([0, -pi / 4, -pi / 3])), transpose(matrix([0, 0, 0]))) T0_RH2 = dot(T0_RH2, Rotate_RH2) h2 = misc.DrawAxes(env, matrix(T0_RH2), 1) #T0_RH2[2,3] += 0.01
def go_to_startik(myRobot, startik): # print "Went to startik" myRobot.SetActiveDOFValues(str2num(startik)) time.sleep(0.05) if (debug): sys.stdin.readline()
# Try to put your feet on the ground print "trying to put the feet on the ground..." whereToFace = MakeTransform(rodrigues([0, 0, -pi / 2]), transpose(matrix([0, 0, 0]))) myIK = put_feet_on_the_ground(robots[0], whereToFace, lowerLimits, upperLimits, env) print "myIK" print myIK if (myIK != ''): print "checking balance constraint..." # print "Press enter to see the result..." # sys.stdin.readline() robots[0].SetDOFValues(str2num(myIK), range(len(robots[0].GetJoints()))) myCOM = array(get_robot_com(robots[0])) myCOM[2, 3] = 0.0 COMHandle = misc.DrawAxes(env, myCOM, 0.3) if (check_support(myCOM, robots[0])): print "This solution is valid" print "left hand path [s:t]: " for pe in candidates[0][collisionFreeSolutions[nxt]]: print str(pe.sIdx), ' : ', str(pe.tIdx) print "right hand path [s:t]: " for pe in candidates[1][collisionFreeSolutions[nxt]]: print str(pe.sIdx), ' : ', str(pe.tIdx) print "Press enter to proceed with the next solution..." sys.stdin.readline()
currentIk = robots[0].GetDOFValues() print currentIk # Try to put your feet on the ground print "trying to put the feet on the ground..." whereToFace = MakeTransform(rodrigues([0, 0, -pi / 2]), transpose(matrix([0, 0, 0]))) myIK = put_feet_on_the_ground(robots[0], whereToFace, lowerLimits, upperLimits, env) print "myIK" print myIK if myIK != "": print "checking balance constraint..." # print "Press enter to see the result..." # sys.stdin.readline() robots[0].SetDOFValues(str2num(myIK), range(len(robots[0].GetJoints()))) myCOM = array(get_robot_com(robots[0])) myCOM[2, 3] = 0.0 COMHandle = misc.DrawAxes(env, myCOM, 0.3) if check_support(myCOM, robots[0]): print "This solution is valid" print "left hand path [s:t]: " for pe in candidates[0][collisionFreeSolutions[nxt]]: print str(pe.sIdx), " : ", str(pe.tIdx) print "right hand path [s:t]: " for pe in candidates[1][collisionFreeSolutions[nxt]]: print str(pe.sIdx), " : ", str(pe.tIdx) print "Press enter to proceed with the next solution..." sys.stdin.readline() else:
# Clean the trajectory files so that we don't execute an old trajectory for i in range(10): try: print "Removing movetraj"+str(i)+".txt" os.remove("movetraj"+str(i)+".txt") except OSError, e: print e ####################################### ## CREATE TRAJ 0: INIT-->APPROACH 1 ## ####################################### print "Press enter to generate init --> approach-1 (trajectory0)" sys.stdin.readline() goaljoints = str2num(self.approachik_1); TSRString_A1_R = SerializeTSR(5,'NULL',self.T0_RH1_APPROACH_1,eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0])) TSRString_A1_L = SerializeTSR(7,'NULL',self.T0_LH1_APPROACH_1,eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0])) TSRChainString_A1 = SerializeTSRChain(0,0,1,1,TSRString_A1_R,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRString_A1_L,'NULL',[]) try: answer = self.probs[0].SendCommand('RunCBiRRT jointgoals %d %s %s'%(len(goaljoints),Serialize1DMatrix(matrix(goaljoints)),TSRChainString_A1)) print "runcbirrt successful" print answer if (answer == '0'): return [] except openrave_exception, e: print "Cannot send command runcbirrt" print e # Rename the file so that we can keep the data w/o overwriting it with a new trajectory
def run(leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs, rm, T0_LH, T0_RH): # Search for the pattern in the reachability model and get the results numRobots = 1 # Keep the number of manipulators of the robots # that are going to be involved in search() # The length of this list should be the same with # the number of robots involved. numManips = [2] T0_p = MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.0]))) myPatterns = [] # 2. Create a search pattern from the trajectory for the first manipulator myPattern = SearchPattern(leftTraj) myPattern.T0_p = T0_p myPattern.setColor(array((0,0,1,0.5))) myPattern.show(env) # sys.stdin.readline() myPattern.hide("all") myPatterns.append(myPattern) # 3. Create a search pattern from the trajectory for the second manipulator myPattern = SearchPattern(rightTraj) myPattern.T0_p = T0_p myPattern.setColor(array((1,0,0,0.5))) myPattern.show(env) # sys.stdin.readline() myPattern.hide("all") myPatterns.append(myPattern) for p in myPatterns: p.hide("spheres") #sys.stdin.readline() for p in myPatterns: p.hide("all") # sys.stdin.readline() T0_starts = [] T0_starts.append(array(T0_LH)) T0_starts.append(array(T0_RH)) mySamples = [] candidates = None print "Ready to look for candidates... ",str(datetime.now()) candidates = look_for_candidates(pairs, rm, myPatterns) if(candidates != None): # find how many candidates search() function found. # candidates[0] is the list of candidate paths for the 0th robot howMany = len(candidates[0]) collisionFreeSolutions = [] valids = [] for c in range(howMany): print "trying ",str(c)," of ",str(howMany)," candidates." allGood = play(T0_starts, relBaseConstraint,candidates,numRobots,numManips,c,myRmaps,[robot],h,env,0.0) h.pop() # delete the robot base axis we added last # We went through all our constraints. Is the candidate valid? if(allGood): collisionFreeSolutions.append(c) findAPathEnds = time.time() print "Success! Collision and configuration constraints met." else: print "Constraint(s) not met ." # Went through all candidates print "Found ",str(len(collisionFreeSolutions))," collision-free solutions in ",str(howMany)," candidates." if (len(collisionFreeSolutions) > 0) : # Have we found at least 1 collision free path? for colFreeSolIdx, solIdx in enumerate(collisionFreeSolutions): print "Playing valid solution #: ",str(colFreeSolIdx) play(T0_starts, relBaseConstraint,candidates,numRobots,numManips,solIdx,myRmaps,[robot],h,env,0.3) h.pop() # delete the robot base axis we added last currentIk = robot.GetDOFValues() # Try to put your feet on the ground print "trying to put the feet on the ground..." whereToFace = MakeTransform(rodrigues([0,0,0]),transpose(matrix([0,0,0]))) myIK = put_feet_on_the_ground(myProblem, robot, whereToFace, lowerLimits, upperLimits, env) print "myIK" print myIK if(myIK != ''): print "checking balance constraint..." robot.SetDOFValues(str2num(myIK), range(len(robot.GetJoints()))) myCOM = array(get_robot_com(robot)) myCOM[2,3] = 0.0 COMHandle = misc.DrawAxes(env,myCOM,0.3) if(check_support(myCOM,robot)): mySamples.append([candidates[0][solIdx],candidates[1][solIdx]]) else: print "COM is out of the support polygon" robot.SetDOFValues(currentIk, range(len(robot.GetJoints()))) return mySamples
myManipulationProblem = RaveCreateModule(myEnv, 'Manipulation') try: myEnv.AddModule(myCBiRRTProblem, myRobot.GetName( )) # this string should match to <Robot name="" > in robot.xml myEnv.AddModule(myManipulationProblem, myRobot.GetName( )) # this string should match to <Robot name="" > in robot.xml except openrave_exception, e: print e #grab the object with manipulator 0 myManipulationProblem.SendCommand('setactivemanip index 0') myManipulationProblem.SendCommand('GrabBody name ' + kinBodyToGrab) jointgoalsStr = deepcopy(goalikStr) jointgoalsNum = str2num(jointgoalsStr) try: # answer = myCBiRRTProblem.SendCommand('RunCBiRRT timelimit 5 smoothingitrs 1 jointgoals %s %s %s'%(len(jointgoalsNum),Serialize1DMatrix(mat(jointgoalsNum)),TSRChainString)) answer = myCBiRRTProblem.SendCommand( 'RunCBiRRT timelimit 5 supportlinks 2 ' + footlinknames + ' smoothingitrs ' + str(fastsmoothingitrs) + ' jointgoals ' + str(len(jointgoalsNum)) + ' ' + Serialize1DMatrix(matrix(jointgoalsNum)) + ' ' + TSRChainString) print print "RunCBiRRT answer: ", str(answer) except openrave_exception, e: print "Cannot send command RunCBiRRT: " print e
def run(leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs, rm, T0_LH, T0_RH): # Search for the pattern in the reachability model and get the results numRobots = 1 # Keep the number of manipulators of the robots # that are going to be involved in search() # The length of this list should be the same with # the number of robots involved. numManips = [2] T0_p = MakeTransform(matrix(rodrigues([0, 0, 0])), transpose(matrix([0.0, 0.0, 0.0]))) myPatterns = [] # 2. Create a search pattern from the trajectory for the first manipulator myPattern = SearchPattern(leftTraj) myPattern.T0_p = T0_p myPattern.setColor(array((0, 0, 1, 0.5))) myPattern.show(env) # sys.stdin.readline() myPattern.hide("all") myPatterns.append(myPattern) # 3. Create a search pattern from the trajectory for the second manipulator myPattern = SearchPattern(rightTraj) myPattern.T0_p = T0_p myPattern.setColor(array((1, 0, 0, 0.5))) myPattern.show(env) # sys.stdin.readline() myPattern.hide("all") myPatterns.append(myPattern) for p in myPatterns: p.hide("spheres") #sys.stdin.readline() for p in myPatterns: p.hide("all") # sys.stdin.readline() T0_starts = [] T0_starts.append(array(T0_LH)) T0_starts.append(array(T0_RH)) mySamples = [] candidates = None print "Ready to look for candidates... ", str(datetime.now()) candidates = look_for_candidates(pairs, rm, myPatterns) if (candidates != None): # find how many candidates search() function found. # candidates[0] is the list of candidate paths for the 0th robot howMany = len(candidates[0]) collisionFreeSolutions = [] valids = [] for c in range(howMany): print "trying ", str(c), " of ", str(howMany), " candidates." allGood = play(T0_starts, relBaseConstraint, candidates, numRobots, numManips, c, myRmaps, [robot], h, env, 0.0) h.pop() # delete the robot base axis we added last # We went through all our constraints. Is the candidate valid? if (allGood): collisionFreeSolutions.append(c) findAPathEnds = time.time() print "Success! Collision and configuration constraints met." else: print "Constraint(s) not met ." # Went through all candidates print "Found ", str( len(collisionFreeSolutions)), " collision-free solutions in ", str( howMany), " candidates." if (len(collisionFreeSolutions) > 0): # Have we found at least 1 collision free path? for colFreeSolIdx, solIdx in enumerate(collisionFreeSolutions): print "Playing valid solution #: ", str(colFreeSolIdx) play(T0_starts, relBaseConstraint, candidates, numRobots, numManips, solIdx, myRmaps, [robot], h, env, 0.3) h.pop() # delete the robot base axis we added last currentIk = robot.GetDOFValues() # Try to put your feet on the ground print "trying to put the feet on the ground..." whereToFace = MakeTransform(rodrigues([0, 0, 0]), transpose(matrix([0, 0, 0]))) myIK = put_feet_on_the_ground(myProblem, robot, whereToFace, lowerLimits, upperLimits, env) print "myIK" print myIK if (myIK != ''): print "checking balance constraint..." robot.SetDOFValues(str2num(myIK), range(len(robot.GetJoints()))) myCOM = array(get_robot_com(robot)) myCOM[2, 3] = 0.0 COMHandle = misc.DrawAxes(env, myCOM, 0.3) if (check_support(myCOM, robot)): mySamples.append( [candidates[0][solIdx], candidates[1][solIdx]]) else: print "COM is out of the support polygon" robot.SetDOFValues(currentIk, range(len(robot.GetJoints()))) return mySamples
def play(T0_starts, T0_FACING, relBaseConstraint, candidates, numRobots, numManips, c, myRmaps, robots, h, myEnv, howLong, doGeneralIk, footlinknames=''): # constraints to check relBaseConstOK = True collisionConstOK = True noConfigJump = True # Get the length of the candidate path # First index stands for the robot index, and the second index stands for the candidate path index. We're calling find_random_candidates() function with an argument of 1. Thus there will be only one candidate returned. Let's find it's length. pathLength = min(len(candidates[0][c]), len(candidates[1][c])) # print "numRobots: ",str(numRobots) # print "numManips: ",str(numManips) [goAhead, activeDOFStartConfig] = start(T0_starts, T0_FACING, candidates, numRobots, numManips, c, myRmaps, robots, h, myEnv, doGeneralIk, footlinknames) if (goAhead): # Get their relative Transformation matrix T0_base = [] for myRobotIndex in range(numRobots): for myManipulatorIndex in range(numManips[myRobotIndex]): T0_base.append(robots[myRobotIndex].GetManipulators() [myManipulatorIndex].GetBase().GetTransform()) Trobot0_robot1 = dot(linalg.inv(T0_base[0]), T0_base[1]) # i) Does the candidate satisfy the robot base transform constraint? if (type(relBaseConstraint) == type([])): # if the input argument type is a list then we have bounds # Check translation constraints if ((abs(Trobot0_robot1[0:3, 3].transpose()) > relBaseConstraint[0:3]).any()): relBaseConstOK = False return [False, ''] # Check rotation constraints if (not allclose(Trobot0_robot1[0:3, 0:3], rodrigues(relBaseConstraint[3:6]))): relBaseConstOK = False return [False, ''] elif (type(relBaseConstraint) == type(array( ()) or relBaseConstraint) == type(matrix(()))): # if input argument type is a numpy array then we have an exact transformation #print "relBaseConstraint?" #print allclose(Trobot0_robot1,relBaseConstraint) if (not allclose(Trobot0_robot1, relBaseConstraint)): relBaseConstOK = False return [False, ''] # If the solution meets the base constraint: if (relBaseConstOK): # print "Base Constraints OK." pathConfigs = [[], []] # A 2D List # ii) Check if the solution collision-free throughout the path? # For each path element, go step by step and check prevConfig = [[], []] currentConfig = [[], []] # print "----" for pElementIndex in range(pathLength): # Move the manipulator to this path element for myRobotIndex in range(numRobots): for myManipulatorIndex in range(numManips[myRobotIndex]): currentSphereIndex = candidates[myManipulatorIndex][c][ pElementIndex].sIdx currentTransformIndex = candidates[myManipulatorIndex][ c][pElementIndex].tIdx myRmaps[myManipulatorIndex].go_to( currentSphereIndex, currentTransformIndex) pathConfigs[myManipulatorIndex].append( robots[myRobotIndex].GetDOFValues( robots[myRobotIndex].GetManipulators() [myManipulatorIndex].GetArmIndices())) # DEBUG SECTION FOR SENSING CONFIGURATION JUMP currentConfig[myManipulatorIndex] = robots[ myRobotIndex].GetDOFValues( robots[myRobotIndex].GetManipulators() [myManipulatorIndex].GetArmIndices()) # print "For robot ",str(myRobotIndex)," ||qA-qB||:" # print "path element ",str(pElementIndex) if (prevConfig[myManipulatorIndex] != []): # print "previous config: " # print prevConfig[myRobotIndex] # print "current config: " # print currentConfig[myRobotIndex] qdiff = absolute( subtract(currentConfig[myManipulatorIndex], prevConfig[myManipulatorIndex])) configDistSq = 0 # for each joint do: for j in range(len(qdiff)): configDistSq += pow(qdiff[j], 2) # find ||qA-qB|| euclideanConfigDistance = pow(configDistSq, 0.5) # print "euclidean configuration distance: " # print euclideanConfigDistance # if(euclideanConfigDistance > configurationJumpThreshold): # noConfigJump = False # return False else: #print "skipping path element:" pass # END OF DEBUG SECTION FOR SENSING CONFIGURATION JUMP prevConfig[myManipulatorIndex] = deepcopy( currentConfig[myManipulatorIndex]) # Check collision with self and with the environment if (myEnv.CheckCollision(robots[myRobotIndex]) or robots[myRobotIndex].CheckSelfCollision()): collisionConstOK = False return [False, ''] # After setting manipulator configurations for this # robot, check if the center of mass is close enough # to the center of robot's feet # # TODO: This part is very messy and ugly. # I need to clean it up and make nice function calls. if (doGeneralIk): if (footlinknames == ''): myIK = put_feet_on_the_ground( robots[myRobotIndex], T0_FACING, myEnv) else: myIK = put_feet_on_the_ground( robots[myRobotIndex], T0_FACING, myEnv, footlinknames) if (myIK != ''): # if successful, show it robots[myRobotIndex].SetActiveDOFValues( str2num(myIK)) # print "checking support in play..." if (not check_support( array(get_robot_com(robots[myRobotIndex])), robots[myRobotIndex])): return [False, ''] else: pass # print "in balance - path element: ",str(pElementIndex) #sys.stdin.readline() else: return [False, ''] # If you didn't break yet, wait before the next path element for visualization time.sleep(howLong) for manipIdx, manipConfs in enumerate(pathConfigs): # print "for manip ",str(manipIdx) for confIdx, conf in enumerate(manipConfs): if (confIdx > 0): qdiff = absolute( subtract(conf, manipConfs[confIdx - 1])) confDistSq = 0 for j in range(len(qdiff)): configDistSq += pow(qdiff[j], 2) eConfDist = pow(configDistSq, 0.5) # print "euclidean configuration distance:" # print eConfDist if (eConfDist > configurationJumpThreshold): noConfigJump = False return [False, ''] # If you made it here, # it means no configuration jump, no collision, and # the center of mass was close enough to the center # of robot's feet for all reachability spheres. return [True, activeDOFStartConfig] else: # start() failed return [False, '']
h.append(misc.DrawAxes(env,T0_LF,0.1)) h.append(misc.DrawAxes(env,T0_RF,0.1)) goalik = probs[0].SendCommand('DoGeneralIK exec supportlinks 2 '+footlinknames #+' movecog '+cogtargStr +' nummanips 2 maniptm 2 '+trans_to_str(T0_LF) +' maniptm 3 '+trans_to_str(T0_RF) ) print "goalik" print goalik if(goalik != ''): robots[idx].SetDOFValues(str2num(goalik),range(35)) print "inBalance" print probs[0].SendCommand('CheckSupport supportlinks 2 '+footlinknames) print "let's calculate the COM here..." # https://en.wikipedia.org/wiki/Center_of_mass # R*M = SUM(r_i*m_i) # # R = SUM(r_i*m_i)/M M = 0 rm = 0 for l in robots[idx].GetLinks(): m = l.GetMass() M += m
# print arg2 # print arg3 # print arg4 if( self.StopAtKeyStrokes ): print "Press Enter to find a goalIK" sys.stdin.readline() self.crankid.SetDOFValues([crank_rot],[crankjointind]) goalik = cbirrtHubo.solve('DoGeneralIK exec supportlinks 2 '+footlinknames+' movecog '+arg1+' nummanips 3 maniptm 0 '+arg2+' maniptm 1 '+arg3+' maniptm 2 '+arg4) # print "goalIK" # print goalik self.robotid.SetActiveDOFValues(str2num(goalik)) self.crankid.SetDOFValues([crank_rot],[crankjointind]) if( self.StopAtKeyStrokes ): print "Press Enter to go to startik" sys.stdin.readline() # Get a trajectory from goalik to grasp configuration goaljoints = deepcopy(goalik) for i in range(TSRChainMimicDOF): goaljoints += ' 0' goaljoints = str2num(goaljoints) self.robotid.SetActiveDOFValues(startik) time.sleep(0.5)
# Uncomment to see T0_RH2 handles.append(misc.DrawAxes(env,matrix(T0_RH2),1)) print "Press Enter to find a goalIK" sys.stdin.readline() crankid.SetDOFValues([crank_rot],[crankjointind]) arg1 = str(cogtarg).strip("[]").replace(', ',' ') arg2 = trans_to_str(T0_LH2) arg3 = trans_to_str(T0_RH2) goalik = probs[0].SendCommand('DoGeneralIK exec supportlinks 2 '+footlinknames+' movecog '+arg1+' nummanips 2 maniptm 0 '+arg2+' maniptm 1 '+arg3) robotid.SetActiveDOFValues(str2num(goalik)) crankid.SetDOFValues([crank_rot],[crankjointind]) crankid.SetDOFValues([0],[crankjointind]) robotid.SetActiveDOFValues(initconfig) robotid.SetDOFValues(rhandopenvals,rhanddofs) robotid.SetDOFValues(lhandopenvals,lhanddofs) print "Press Enter to plan initconfig --> startik" sys.stdin.readline() # Get a trajectory from initial configuration to grasp configuration with robotid: try: answer = probs[0].SendCommand('RunCBiRRT psample 0.2 supportlinks 2 '+footlinknames+' smoothingitrs '+str(normalsmoothingitrs)+' '+TSRChainStringGrasping)
os.remove("movetraj" + str(i) + ".txt") except OSError, e: # print e pass ####################################### ## CREATE TRAJ 0: INIT-->APPROACH 1 ## ####################################### self.robot.SetActiveDOFValues(self.initconfig) ############################################# ## CREATE TRAJ 2: APPROACH 2 --> STARTIK ## ############################################# goaljoints = str2num(self.startik) if not self.plan_and_save_trajectory(goaljoints, "movetraj2.txt", False, False): return 0 ####################################### ## CREATE TRAJ 3: STARTIK --> GOALIK ## ####################################### goaljoints = str2num(self.goalik) goaljoints = concatenate((goaljoints, array([0])), axis=1) T0_w0L = MakeTransform(matrix(rodrigues([self.wristPitch, 0, 0])), transpose(matrix([0, 0, 0]))) T0_w0R = MakeTransform( rodrigues([0, self.wristPitch, 0]), matrix([[self.jointtm[9]], [self.jointtm[10]], [self.jointtm[11]]]) )
TSRChainString = SerializeTSRChain(0,0,1,1,TSRstring1,'crank',matrix([crankjointind]))+' '+SerializeTSRChain(0,0,1,1,TSRstring2,'NULL',[]) print TSRChainString # Clean the trajectory files so that we don't execute an old trajectory for i in range(2): try: print "Removing movetraj"+str(i)+".txt" os.remove("movetraj"+str(i)+".txt") except OSError, e: print e # Set the current goal to RH1 and LH1 goaljoints = str2num(startik); #print goaljoints print "Press enter to continue 2.1.." # sys.stdin.readline() self.robot.SetActiveDOFValues(initconfig) self.crankid.SetDOFValues([0],[crankjointind]) #print goaljoints print "Press enter to continue 2.2..." # sys.stdin.readline() TSRString_G1 = SerializeTSR(4,'NULL',T0_RH1,eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0])) TSRString_G2 = SerializeTSR(7,'NULL',T0_LH1,eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0])) TSRChainString_G = SerializeTSRChain(0,0,1,1,TSRString_G1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRString_G2,'NULL',[])
# Note: It turns out the PR2 has 8 manipulators in OpenRAVE # # maniptm 6: leftarm_torso (this coordinate frame is in the middle of the gripper) # maniptm 4: rightarm_torso (this coordinate frame is in the middle of the gripper) # # startik = probs[0].SendCommand('DoGeneralIK exec nummanips 2 maniptm 6 '+arg1+' maniptm 4 '+arg2) print "Got startik:" print startik print "------" # Matlab version has the following line but in Python it doesn't seem to do anything, # Mostly beause probs[0].SendCommand makes the manipulators move already. robotid.SetActiveDOFValues(str2num(startik)) # Note: startik: T0_LH1 and T0_RH1 print "go to startik" sys.stdin.readline() # Note: GetDOFValues() returns all 39 joints' values, whereas GetActiveDOFValues returns only the active joints' values # THIS IS THE END OF THE SECOND BLOCK IN MATLAB VERSION # Left hand is evaluated first, so need to make right hand relative to crank cranklinks = crankid.GetLinks(); # debug # print cranklinks # for cl in range(len(cranklinks)): # print cranklinks[cl].GetName()
Trobot = array([-0.0024,1,0,-1,-0.0024,0,0,0,1,1.2996,-0.6217,0.000]).reshape(4,3).T robot.SetTransform(Trobot) robot.SetActiveDOFs(r_[0, 1, 2, 3, 4, 5, 6, 11, 12, 13, 14, 15, 16, 17]) robot.SetActiveDOFValues(initdofvals) handdof = r_[(2*ones([1,3]))[0]]; robot.SetActiveDOFs([7, 8, 9]) robot.SetActiveDOFValues(handdof) #set the active dof robot.SetActiveDOFs(activedofs) #get info about the fridge hinge fridgejointind = 6 jointtm = str2num(probs_cbirrt.SendCommand('GetJointTransform name kitchen jointind %d'%fridgejointind)) #set up the grasp T0_RH1 = MakeTransform(rodrigues([0,0,pi])*(mat([-1, 0, 0, 0, 0, -1, 0, -1, 0]).T).reshape(3,3), mat([1.4351, -0.21, 1.1641]).T) startik = str2num(probs_cbirrt.SendCommand('DoGeneralIK exec nummanips 1 maniptm 0 %s'%SerializeTransform(T0_RH1))) robot.SetActiveDOFValues(startik) time.sleep(0.5) #let the simulator draw the scene #define TSR chains #place the first TSR's reference frame at the door's hinge with no rotation relative to world frame T0_w0 = MakeTransform(rodrigues([0,0,pi])*(mat([1, 0, 0, 0, 1, 0, 0, 0, 1]).T).reshape(3,3),mat(jointtm[9:12]).T)
handrot0 = mat(rodrigues([-pi/2, 0, 0])) handtrans1 = T0_object[0:3,3] + transoffset1; handrot1 = mat(rodrigues([pi/2, 0, 0])) Tik0 = MakeTransform(handrot0,handtrans0); Tik1 = MakeTransform(handrot1,handtrans1); #get the ik solutions for both arms for the box in the start pose robot.SetActiveDOFs(arm0dofs) startik0 = probs_cbirrt.SendCommand('DoGeneralIK exec nummanips 1 maniptm 0 %s'%SerializeTransform(Tik0)) robot.SetActiveDOFs(arm1dofs) startik1 = probs_cbirrt.SendCommand('DoGeneralIK exec nummanips 1 maniptm 1 %s'%SerializeTransform(Tik1)) startik = '%s %s'%(startik0,startik1) robot.SetActiveDOFs(activedofs) robot.SetActiveDOFValues(str2num(startik)) #define the target transform of the goal T0_object2 = MakeTransform(mat(eye(3)),mat([0.6923,0.00,1.3989]).T) targobject.SetTransform(array(T0_object2[0:3][:,0:4])) #define targets for both hands relative to box in goal pose handtrans0 = T0_object2[0:3,3] + transoffset0; handtrans1 = T0_object2[0:3,3] + transoffset1; Tik0 = MakeTransform(handrot0,handtrans0); Tik1 = MakeTransform(handrot1,handtrans1);
self.robotid.WaitForController(0) self.robotid.GetController().Reset(0) self.robotid.SetDOFValues(self.rhandclosevals, self.rhanddofs) self.robotid.SetDOFValues(self.lhandclosevals, self.lhanddofs) if (self.StopAtKeyStrokes): print "Press Enter to plan startik --> goalik " sys.stdin.readline() # Get a trajectory from goalik to grasp configuration goaljoints = deepcopy(goalik) for i in range(TSRChainMimicDOF): goaljoints += ' 0' goaljoints = str2num(goaljoints) try: answer = probs[0].SendCommand( 'RunCBiRRT supportlinks 2 ' + footlinknames + ' smoothingitrs ' + str(fastsmoothingitrs) + ' jointgoals ' + str(len(goaljoints)) + ' ' + Serialize1DMatrix(matrix(goaljoints)) + ' ' + TSRChainString) print "RunCBiRRT answer: ", str(answer) except openrave_exception, e: print "Cannot send command RunCBiRRT: " print e try: os.rename("cmovetraj.txt", "movetraj1.txt") except OSError, e: