def handle_update_valve_pose(self,req): wheel = req.valve print "wheel pose.position" print wheel.pose.position r = req.radius print "Radius" print r T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame Ttll_wheel = MakeTransform(rotationMatrixFromQuat([wheel.pose.orientation.w,wheel.pose.orientation.x,wheel.pose.orientation.y,wheel.pose.orientation.z]),matrix([wheel.pose.position.x,wheel.pose.position.y,wheel.pose.position.z])) # Transform from torso_lift_link to racing wheel's frame # Wheel's model is defined rotated, the following transform will make it look straight up wheelStraightUp = MakeTransform(matrix(dot(rodrigues([0,0,-pi/2]),rodrigues([pi/2,0,0]))),transpose(matrix([0,0,0]))) wheelStraightUp = dot(wheelStraightUp,MakeTransform(rodrigues([-0.35,0,0]),transpose(matrix([0,0,0])))) # For some reason we need to correct the 3D sketchup model for the real life wheel angle in the lab. This is the offset. wheelAtCorrectLocation = dot(T0_tll,Ttll_wheel) # Wheel's pose in world coordinates, adjusted for torso lift link transform wheelAtCorrectLocation = dot(wheelAtCorrectLocation,wheelStraightUp) # Rotate the model so that it looks straight up self.crankid.SetTransform(array(wheelAtCorrectLocation)) res = updateValvePosResponse() cx = wheel.pose.position.x # current x value for the valve in torso_lift_link cy = wheel.pose.position.y # current y value for the valve in torso_lift_link print "cx" print cx print "cy" print cy d = [cx,cy] # delta print "delta" print d self.moveArmsToInitPosition() rot_res = self.rotate_base(d) if( rot_res == 1): appr_res = self.approach_base(d) if(appr_res == 1): # Now we know where the valve is located, let's run the test points if(self.generate_points() == 1): # If tests succeeded go on creating the trajectories if(self.arm_planner() == 1): # Finally finish by playing the trajectories and return the code task_control_response = pr2_rave2task_control() # This function packs movetraj files in 3 packages and sends them over to task_control. res.success_code = task_control_response.result # Just return whatever the task control is returning back to GUI else: res.success_code = 'arm_planner failed' else: res.success_code = 'generate_points failed' else: res.success_code = 'approach is not good enough. align again.' elif(rot_res == 2): res.success_code = 'rotation is not good enough. align again.' print 'End of pipeline - result:' print res.success_code return res
def run(): normalsmoothingitrs = 150; fastsmoothingitrs = 20; env = Environment() RaveSetDebugLevel(DebugLevel.Info) # set output level to debug env.Load('../../models/driving_wheel.robot.xml') robotid = env.ReadRobotURI('robots/pr2-beta-static.zae') crankid = env.GetRobots()[0] crankid.SetTransform(array(MakeTransform(matrix(dot(rodrigues([0,0,pi/2]),rodrigues([pi/2,0,0]))), transpose(matrix([0.5,0,0.8128]))))) env.Add(robotid) env.SetViewer('qtcoin') probs_cbirrt = RaveCreateModule(env,'CBiRRT') probs_crankmover = RaveCreateModule(env,'CBiRRT') manips = robotid.GetManipulators() crankmanip = crankid.GetManipulators() try: env.AddModule(probs_cbirrt,'pr2') env.AddModule(probs_crankmover,'crank') except openrave_exception, e: print e
def handle_update_valve_pose(self,req): wheel = req.valve print "wheel pose.position" print wheel.pose.position r = req.radius print "Radius" print r T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame Ttll_wheel = MakeTransform(rotationMatrixFromQuat([wheel.pose.orientation.w,wheel.pose.orientation.x,wheel.pose.orientation.y,wheel.pose.orientation.z]),matrix([wheel.pose.position.x,wheel.pose.position.y,wheel.pose.position.z])) # Transform from torso_lift_link to racing wheel's frame # Wheel's model is defined rotated, the following transform will make it look straight up wheelStraightUp = MakeTransform(matrix(dot(rodrigues([0,0,-pi/2]),rodrigues([pi/2,0,0]))),transpose(matrix([0,0,0]))) wheelStraightUp = dot(wheelStraightUp,MakeTransform(rodrigues([-0.35,0,0]),transpose(matrix([0,0,0])))) # For some reason we need to correct the 3D sketchup model for the real life wheel angle in the lab. This is the offset. wheelAtCorrectLocation = dot(T0_tll,Ttll_wheel) # Wheel's pose in world coordinates, adjusted for torso lift link transform wheelAtCorrectLocation = dot(wheelAtCorrectLocation,wheelStraightUp) # Rotate the model so that it looks straight up self.crankid.SetTransform(array(wheelAtCorrectLocation)) res = updateValvePosResponse() res.success_code = "yeeha" return res
def wheel_pose_callback(self, wheelPose): T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame Ttll_wheel = MakeTransform( rotationMatrixFromQuat( [ wheelPose.pose.orientation.w, wheelPose.pose.orientation.x, wheelPose.pose.orientation.y, wheelPose.pose.orientation.z, ] ), matrix([wheelPose.pose.position.x, wheelPose.pose.position.y, wheelPose.pose.position.z]), ) # Transform from torso_lift_link to racing wheel's frame # Wheel's model is defined rotated, the following transform will make it look straight up wheelStraightUp = MakeTransform( matrix(dot(rodrigues([0, 0, -pi / 2]), rodrigues([pi / 2, 0, 0]))), transpose(matrix([0, 0, 0])) ) wheelAtCorrectLocation = dot( T0_tll, Ttll_wheel ) # Wheel's pose in world coordinates, adjusted for torso lift link transform wheelAtCorrectLocation = dot( wheelAtCorrectLocation, wheelStraightUp ) # Rotate the model so that it looks straight up self.crankid.SetTransform(array(wheelAtCorrectLocation))
def openrave_only_set_wheel_pose(self): T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame Ttll_wheel = MakeTransform( rotationMatrixFromQuat([-0.0434053950012, 0.205337584019, 0.0024010904599, 0.977725327015]), matrix([0.561164438725, 0.00322353374213, 0.101688474417]), ) # Transform from torso_lift_link to racing wheel's frame # Wheel's model is defined rotated, the following transform will make it look straight up wheelStraightUp = MakeTransform( matrix(dot(rodrigues([0, 0, -pi / 2]), rodrigues([pi / 2, 0, 0]))), transpose(matrix([0, 0, 0])) ) wheelStraightUp = dot( wheelStraightUp, MakeTransform(rodrigues([-0.4, 0, 0]), transpose(matrix([0, 0, 0]))) ) # For some reason we need to correct the 3D sketchup model for the real life wheel angle in the lab. This is the offset. wheelAtCorrectLocation = dot( T0_tll, Ttll_wheel ) # Wheel's pose in world coordinates, adjusted for torso lift link transform wheelAtCorrectLocation = dot( wheelAtCorrectLocation, wheelStraightUp ) # Rotate the model so that it looks straight up self.crankid.SetTransform(array(wheelAtCorrectLocation))
def run(): normalsmoothingitrs = 150; fastsmoothingitrs = 20; env = Environment() RaveSetDebugLevel(DebugLevel.Info) # set output level to debug env.Load('../../models/driving_wheel.robot.xml') robotid = env.ReadRobotURI('robots/pr2-beta-static.zae') crankid = env.GetRobots()[0] # Move the wheel to the front of the robot, somewhere it can grasp and rotate # Note that when mounted on our foldable table the height of the wheel is 0.8128 crankid.SetTransform(array(MakeTransform(matrix(dot(rodrigues([0,0,pi/2]),rodrigues([pi/2,0,0]))), transpose(matrix([0.5,0,0.8128]))))) env.Add(robotid) env.SetViewer('qtcoin') # Note: RaveCreateProblem will be deprecated and it uses RaveCrateModule in it anyways so we just use RaveCreateModule instead probs_cbirrt = RaveCreateModule(env,'CBiRRT') probs_crankmover = RaveCreateModule(env,'CBiRRT') manips = robotid.GetManipulators() crankmanip = crankid.GetManipulators() # for m in range(len(manips)): # print manips[m].armjoints # Note: The argument you pass in must be the same as (the robot name) defined in the .zae or .xml file # Try - Catch is very useful to figure out what the problem is (if any) with cbirrt function calls try: env.AddModule(probs_cbirrt,'pr2') env.AddModule(probs_crankmover,'crank') except openrave_exception, e: print e
def wheel_pose_callback(self,wheelPose): if(self.notPlanning == True): # We don't want the wheel to move during trajectory planning T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame Ttll_wheel = MakeTransform(rotationMatrixFromQuat([wheelPose.pose.orientation.w,wheelPose.pose.orientation.x,wheelPose.pose.orientation.y,wheelPose.pose.orientation.z]),matrix([wheelPose.pose.position.x,wheelPose.pose.position.y,wheelPose.pose.position.z])) # Transform from torso_lift_link to racing wheel's frame wheelStraightUp = MakeTransform(matrix(dot(rodrigues([0,0,-pi/2]),rodrigues([pi/2,0,0]))),transpose(matrix([0,0,0]))) wheelAtCorrectLocation = dot(T0_tll,Ttll_wheel) wheelAtCorrectLocation = dot(wheelAtCorrectLocation,wheelStraightUp) self.crankid.SetTransform(array(wheelAtCorrectLocation))
def callback(self,wheelPose): T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame Ttll_wheel = MakeTransform(rotationMatrixFromQuat([wheelPose.pose.orientation.w,wheelPose.pose.orientation.x,wheelPose.pose.orientation.y,wheelPose.pose.orientation.z]),matrix([wheelPose.pose.position.x,wheelPose.pose.position.y,wheelPose.pose.position.z])) # Transform from torso_lift_link to racing wheel's frame #wheel_x = torso_lift_link[0][3]+data.data[0] #wheel_y = torso_lift_link[1][3]+data.data[1] #wheel_z = torso_lift_link[2][3]+data.data[2] #w=[wheel_x,wheel_y,wheel_z] wheelStraightUp = MakeTransform(matrix(dot(rodrigues([0,0,-pi/2]),rodrigues([pi/2,0,0]))),transpose(matrix([0,0,0]))) wheelAtCorrectLocation = dot(T0_tll,Ttll_wheel) wheelAtCorrectLocation = dot(wheelAtCorrectLocation, wheelStraightUp) self.crankid.SetTransform(array(wheelAtCorrectLocation))
def save_markers_to_file(self): with open("./matlab/markers_tmp.csv", 'w') as m_file: line_str = "" # Construct frame centered at torso with orientation # set by the pelvis frame, add rotation offset for matlab code t_trans = deepcopy(self.t_pelvis) t_trans[0:3, 3] = deepcopy(self.t_torso[0:3, 3]) t_trans = t_trans * \ MakeTransform(rodrigues([0, 0, pi]), matrix([0, 0, 0])) # inv_pelvis = la.inv(self.t_pelvis) # self.handles.append(misc.DrawAxes(self.env, inv_pelvis * t_trans, 2)) t_trans = la.inv(t_trans) for marker in self.markers: marker = array(array(t_trans).dot(append(marker, 1)))[0:3] line_str += str(marker[0] * 1000) + ',' line_str += str(marker[1] * 1000) + ',' line_str += str(marker[2] * 1000) + ',' line_str = line_str.rstrip(',') line_str += '\n' m_file.write(line_str)
def get_rotate_cw(minRotAngle, maxRotAngle, delta1, delta2, r): rotTrajsL = [] # rotational trajectories for the left hand rotTrajsR = [] # rotational trajectories for the right hand for angle in frange(minRotAngle, maxRotAngle, delta1): currentTrajL = [] currentTrajR = [] xyrL = RotationalTrajDiscretizer.get_discrete_xyr( r, pi, pi - angle, -0.01) rawL = xyrL[0] discreteL = xyrL[1] #print "got discrete elements - L" for element in discreteL: #print element adjustedY = -1.0 * (element[0] + r) + 0.0 adjustedZ = element[1] adjustedR = (4.0 - element[2]) * (pi / 4.0) currentTrajL.append( array( MakeTransform( matrix(rodrigues([adjustedR, 0, 0])), transpose(matrix([0.0, adjustedY, adjustedZ]))))) #print str(adjustedY), str(adjustedZ), str(adjustedR) xyrR = RotationalTrajDiscretizer.get_discrete_xyr( r, 0.0, -1.0 * angle, -0.01) rawR = xyrR[0] discreteR = xyrR[1] # print "got discrete elements - R" for element in discreteR: # print element adjustedY = r - element[0] adjustedZ = element[1] adjustedR = (-1.0 * element[2] + 0.0) * (pi / 4.0) # print str(adjustedY), str(adjustedZ), str(adjustedR) currentTrajR.append( array( MakeTransform( matrix(rodrigues([adjustedR, 0, 0])), transpose(matrix([0.0, adjustedY, adjustedZ]))))) rotTrajsL.append(currentTrajL) rotTrajsR.append(currentTrajR) return [rotTrajsL, rotTrajsR]
def callback(self,data): #self.crankid.SetTransform(array(MakeTransform(matrix(dot(rodrigues([0,0,pi/2]),rodrigues([pi/2,0,0]))), transpose(matrix([0.5,0,0.8128]))))) torso_lift_link = self.robot.GetLinks()[16].GetTransform() wheel_x = torso_lift_link[0][3]+data.data[0] wheel_y = torso_lift_link[1][3]+data.data[1] wheel_z = torso_lift_link[2][3]+data.data[2] w=[wheel_x,wheel_y,wheel_z] self.crankid.SetTransform(array(MakeTransform(matrix(dot(rodrigues([0,0,pi/2]),rodrigues([pi/2,0,0]))), transpose(matrix(w))))) print "4"
def get_push(minTrajLength,maxTrajLength, delta1, delta2): pushTrajsL = [] for length in frange(minTrajLength,maxTrajLength,delta1): currentTraj = [] for l in frange(0,length,delta2): currentTraj.append(array(MakeTransform(matrix(rodrigues([0, 0, 0])),transpose(matrix([l, 0.0, 0.0]))))) pushTrajsL.append(currentTraj) pushTrajsR = deepcopy(pushTrajsL) return [pushTrajsL, pushTrajsR]
def get_lift(minTrajLength, maxTrajLength, delta1, delta2): liftTrajsL = [] for length in frange( minTrajLength, maxTrajLength, delta1): currentTraj = [] for l in frange(0, length, delta2): currentTraj.append(array(MakeTransform(matrix(rodrigues([0, 0, 0])),transpose(matrix([0.0, 0.0, l]))))) liftTrajsL.append(currentTraj) liftTrajsR = deepcopy(liftTrajsL) return [liftTrajsL, liftTrajsR]
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
def run(candidates, leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs, rm, T0_LH, T0_RH, relBaseConstraint): mySamples = [] # 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_starts = [] T0_starts.append(array(T0_LH)) T0_starts.append(array(T0_RH)) whereToFace = MakeTransform(rodrigues([0,0,0]),transpose(matrix([0,0,0]))) # 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, activeDOFStartConfigStr] = play(T0_starts, whereToFace, relBaseConstraint, candidates, numRobots, numManips, c ,myRmaps,[robot], h, env, 0.0, True, ' leftFootBase rightFootBase ') h.pop() # delete the robot base axis we added last # We went through all our constraints. Is the candidate valid? if(allGood): collisionFreeSolutions[0].append(c) collisionFreeSolutions[1].append(activeDOFStartConfigStr) 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[0]))," collision-free solutions in ",str(howMany)," candidates." if (len(collisionFreeSolutions[0]) > 0) : print "FOUND COLLISION FREE SOLUTIONS!!! WHOOHOO!!!! WILL PLAY RESULTS!!!" # sys.stdin.readline() # Have we found at least 1 collision free path? for colFreeSolIdx, solIdx in enumerate(collisionFreeSolutions[0]): mySamples.append([candidates[0][solIdx],candidates[1][solIdx],collisionFreeSolutions[1][colFreeSolIdx]]) return mySamples
def get_lift(minTrajLength, maxTrajLength, delta1, delta2): liftTrajsL = [] for length in frange(minTrajLength, maxTrajLength, delta1): currentTraj = [] for l in frange(0, length, delta2): currentTraj.append( array( MakeTransform(matrix(rodrigues([0, 0, 0])), transpose(matrix([0.0, 0.0, l]))))) liftTrajsL.append(currentTraj) liftTrajsR = deepcopy(liftTrajsL) return [liftTrajsL, liftTrajsR]
def get_rotate_cw( minRotAngle, maxRotAngle, delta1, delta2, r): rotTrajsL = [] # rotational trajectories for the left hand rotTrajsR = [] # rotational trajectories for the right hand for angle in frange( minRotAngle, maxRotAngle, delta1): currentTrajL = [] currentTrajR = [] xyrL = RotationalTrajDiscretizer.get_discrete_xyr(r, pi, pi-angle, -0.01) rawL = xyrL[0] discreteL = xyrL[1] #print "got discrete elements - L" for element in discreteL: #print element adjustedY = -1.0*(element[0]+r)+0.0 adjustedZ = element[1] adjustedR = (4.0 - element[2])*(pi/4.0) currentTrajL.append(array(MakeTransform(matrix(rodrigues([adjustedR, 0, 0])),transpose(matrix([0.0, adjustedY, adjustedZ]))))) #print str(adjustedY), str(adjustedZ), str(adjustedR) xyrR = RotationalTrajDiscretizer.get_discrete_xyr(r, 0.0, -1.0*angle, -0.01) rawR = xyrR[0] discreteR = xyrR[1] # print "got discrete elements - R" for element in discreteR: # print element adjustedY = r-element[0] adjustedZ = element[1] adjustedR = (-1.0*element[2]+0.0)*(pi/4.0) # print str(adjustedY), str(adjustedZ), str(adjustedR) currentTrajR.append(array(MakeTransform(matrix(rodrigues([adjustedR, 0, 0])),transpose(matrix([0.0, adjustedY, adjustedZ]))))) rotTrajsL.append(currentTrajL) rotTrajsR.append(currentTrajR) return [rotTrajsL, rotTrajsR]
def get_push(minTrajLength, maxTrajLength, delta1, delta2): pushTrajsL = [] for length in frange(minTrajLength, maxTrajLength, delta1): currentTraj = [] for l in frange(0, length, delta2): currentTraj.append( array( MakeTransform(matrix(rodrigues([0, 0, 0])), transpose(matrix([l, 0.0, 0.0]))))) pushTrajsL.append(currentTraj) pushTrajsR = deepcopy(pushTrajsL) return [pushTrajsL, pushTrajsR]
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
def get_robot_com(myRobot): # print "let's calculate the COM here..." # # This is how the center of mass is calculated # # 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 myRobot.GetLinks(): # mass of this link m = l.GetMass() # keep the total mass of the robot M += m # l.GetTransform() does not return the center of mass of the link r = dot( l.GetTransform(), array( MakeTransform(rodrigues([0, 0, 0]), transpose(matrix(l.GetCOMOffset()))))) # multiplication by element rm += multiply(r, m) # print "total mass:" # print M # division by element R = divide(rm, M) # print "T0_COM for "+myRobot.GetName() # print R.round(4) # Draw the center of mass if you want # h.append(misc.DrawAxes(myEnv,array(R.round(4)),0.1)) return R
def get_candidates(leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs, rm): 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 myPatterns.append(myPattern) # 3. Create a search pattern from the trajectory for the second manipulator myPattern = SearchPattern(rightTraj) myPattern.T0_p = T0_p myPatterns.append(myPattern) candidates = None print "Ready to look for candidates... ",str(datetime.now()) candidates = look_for_candidates(pairs, rm, myPatterns) return candidates
def find_neighbors(self): print "Finding neighbors..." # For all spheres in the map do: for index, sphere in enumerate(self.map): sphere.neighbors = [] # For each possible neighbor do: for Tsphere_neighbor in self.Tsphere_neighbors: # Erase rotation (we don't care), keep transformation. Tbase_sphere = MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([sphere.T[0][0,3],sphere.T[0][1,3],sphere.T[0][2,3]]))) # Convert the transform into robot's base coordinates Tbase_neighbor = dot(Tbase_sphere,Tsphere_neighbor) # Convert the coordinates into a string key = str(round(Tbase_neighbor[0,3],2)),",",str(round(Tbase_neighbor[1,3],2)),",",str(round(Tbase_neighbor[2,3],2)) # If there is a reachability sphere at these coordinates if( key in self.indices ): # Look-up the index of the neighbor and keep it sphere.neighbors.append(self.indices[key]) print "Finding neighbors, done."
def get_candidates(leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs, rm): 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 myPatterns.append(myPattern) # 3. Create a search pattern from the trajectory for the second manipulator myPattern = SearchPattern(rightTraj) myPattern.T0_p = T0_p myPatterns.append(myPattern) candidates = None print "Ready to look for candidates... ", str(datetime.now()) candidates = look_for_candidates(pairs, rm, myPatterns) return candidates
def get_robot_com(myRobot): # print "let's calculate the COM here..." # # This is how the center of mass is calculated # # 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 myRobot.GetLinks(): # mass of this link m = l.GetMass() # keep the total mass of the robot M += m # l.GetTransform() does not return the center of mass of the link r = dot(l.GetTransform(),array(MakeTransform(rodrigues([0,0,0]),transpose(matrix(l.GetCOMOffset()))))) # multiplication by element rm += multiply(r,m) # print "total mass:" # print M # division by element R = divide(rm,M) # print "T0_COM for "+myRobot.GetName() # print R.round(4) # Draw the center of mass if you want # h.append(misc.DrawAxes(myEnv,array(R.round(4)),0.1)) return R
def get_markers_in_pelvis_frame(self, markers, t_pelvis): # Construct frame centered at torso with orientation # set by the pelvis frame, add rotation offset for matlab code trunk_center = (markers[0] + markers[1]) / 2 self.t_trans = deepcopy(t_pelvis) self.t_trans[0:3, 3] = transpose(matrix(trunk_center)) self.t_trans = self.t_trans * \ MakeTransform(rodrigues([0, 0, pi]), matrix([0, 0, 0])) inv_t_trans = la.inv(self.t_trans) points_3d = len(markers) * [array([0., 0., 0.])] for i, p in enumerate(markers): points_3d[i] = array( array(inv_t_trans).dot(array(append(p, 1.0))))[0:3] # points_3d[i] *= 1000 return points_3d
def get_candidates(leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs, rm): 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() candidates = None print "Ready to look for candidates... ", str(datetime.now()) candidates = look_for_candidates(pairs, rm, myPatterns) return candidates
def get_candidates(leftTraj, rightTraj, env, robot, myRmaps, myProblem, pairs, rm): 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() candidates = None print "Ready to look for candidates... ",str(datetime.now()) candidates = look_for_candidates(pairs, rm, myPatterns) return candidates
# The following two matrices define no rotation. Only translation # T0_LH1=matrix([[1,0,0,0.5],[0,1,0,0.3],[0,0,1,1.2],[0,0,0,1]]) # T0_RH1=matrix([[1,0,0,0.5],[0,1,0,-0.3],[0,0,1,1.1],[0,0,0,1]]) # The following two matrices define translation and rotation of 90 degrees around WORLD's x-axis and the new coordinate frame's Z-axis # # For rotation around X, left hand should be rotated in the positive direction, whereas right hand should be rotated in the negative # For rotation around Y both hands should be rotated in the negative direction # KEEP THESE JUST IN CASE UNTIL YOU DEBUG #T0_LH1 = MakeTransform(array(dot(rodrigues([pi/2,0,0]),rodrigues([0,0,pi/2]))),array([[0.51],[0.11],[0.8]])) #T0_RH1 = MakeTransform(array(dot(rodrigues([-pi/2,0,0]),rodrigues([0,0,pi/2]))),array([[0.51],[-0.11],[0.8]])) # 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])))) # Translate the new coordinate frame -0.11 meters on its Z-axis and assign it to Left Hand Left_Hand_Point_In_Wheel_Coordinate_Frame = dot(temp,MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0,0,-0.11])))) T0_LH1 = Left_Hand_Point_In_Wheel_Coordinate_Frame # 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]))))
def handle_update_valve_pose(self, req): # This is the main cycle that is triggered by RViz when the user clicks on Send2Robot # Get the valve pose wheel = req.valve print "GUI sends the following:" print wheel # Get the valve radius r = req.radius T0_tll = self.robot.GetLinks()[16].GetTransform() # Transform from the origin to torso_lift_link frame Ttll_wheel = MakeTransform( rotationMatrixFromQuat( [wheel.pose.orientation.w, wheel.pose.orientation.x, wheel.pose.orientation.y, wheel.pose.orientation.z] ), matrix([wheel.pose.position.x, wheel.pose.position.y, wheel.pose.position.z]), ) # Transform from torso_lift_link to racing wheel's frame # Wheel's model is defined rotated, the following transform will make it look straight up wheelStraightUp = MakeTransform( matrix(dot(rodrigues([0, 0, -pi / 2]), rodrigues([pi / 2, 0, 0]))), transpose(matrix([0, 0, 0])) ) wheelStraightUp = dot( wheelStraightUp, MakeTransform(rodrigues([-0.4, 0, 0]), transpose(matrix([0, 0, 0]))) ) # For some reason we need to correct the 3D sketchup model for the real life wheel angle in the lab. This is the offset. wheelAtCorrectLocation = dot( T0_tll, Ttll_wheel ) # Wheel's pose in world coordinates, adjusted for torso lift link transform wheelAtCorrectLocation = dot( wheelAtCorrectLocation, wheelStraightUp ) # Rotate the model so that it looks straight up self.crankid.SetTransform(array(wheelAtCorrectLocation)) res = updateValvePosResponse() cx = wheel.pose.position.x # current x value for the valve in torso_lift_link cy = wheel.pose.position.y # current y value for the valve in torso_lift_link d = [cx, cy] # delta self.moveArmsToInitPosition() self.planning_lock.acquire() # rot_res = self.rotate_base(d) cbirrt_result = -1 skip = -1 user_note = "" if self.generate_points() == 1: # print"generate_points succeeded" if self.arm_planner() == 1: cbirrt_result = 1 # res.success_code = "not using task_control." # print "skip?" # ans = sys.stdin.readline() # if(ans[0] == 'y'): # skip = 1 # res.success_code = "skipped" # else: # skip = 0 # # print"arm planner succeeded" # # Finally finish by playing the trajectories and return the code task_control_response = pr2_rave2task_control( req.id ) # This function packs movetraj files in 3 packages and sends them over to task_control. res.success_code = ( task_control_response.result ) # Just return whatever the task control is returning back to GUI else: cbirrt_result = 0 res.success_code = "arm planner failed" else: res.success_code = "generate points failed" self.planning_lock.release() if self.save_log: print "Iteration #" + str(self.num_iteration) # print "Enter your notes for this iteration:" # user_note = sys.stdin.readline() ts = str(datetime.now())[11:19] data = [ self.num_iteration, cbirrt_result, req.id, wheel.pose.position.x, wheel.pose.position.y, wheel.pose.position.z, wheel.pose.orientation.x, wheel.pose.orientation.y, wheel.pose.orientation.z, wheel.pose.orientation.w, req.rms_err, req.angle_err, res.success_code.replace(",", "|"), skip, ts, user_note[0 : len(user_note) - 1], # leave the \n in the end out ] self.my_logger.save(data) self.num_iteration = self.num_iteration + 1 print "End of pipeline - result:" print res.success_code return res
## 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]]]) ) Tw0_eL = dot(linalg.inv(self.T0_crankcrank), self.T0_LH1_ENTRY) Tw0_eR = dot(linalg.inv(T0_w0R), self.T0_RH1_ENTRY) Bw0L = matrix([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) Bw0R = matrix([0, 0, 0, 0, 0, 0, -pi, pi, 0, 0, 0, 0]) TSRString_G_R = SerializeTSR(5, "NULL", T0_w0R, Tw0_eR, Bw0R) # TSR for the right gripper TSRString_G_L = SerializeTSR(7, "crank crank", T0_w0L, Tw0_eL, Bw0L) # TSR for the left gripper TSRChainString_StoG = ( SerializeTSRChain(0, 0, 1, 1, TSRString_G_R, "crank", matrix([self.crankjointind])) + " "
# print "traj call answer: ",str(answer) # except openrave_exception, e: # print e # Get the current configuration of the robot # and assign it to startik (start of the wheel # rotation path). startik = self.robotid.GetActiveDOFValues() # Left Hand's index is less than the right hand. # Hence it is evaluated first by the CBiRRT Module. # That's why We need to define the right hand's # transform relative to the wheel (ask Dmitry Berenson # about this for more information). temp1 = MakeTransform(rodrigues([-pi/2,0,0]),transpose(matrix([0,0,0]))) temp2 = MakeTransform(rodrigues([0,0,-pi/2]),transpose(matrix([0,0,0]))) # 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 = dot(dot(CTee,temp1),temp2) # 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
def play(relBaseConstraint,candidates,numRobots,c,myRmaps,robots,h,env): # constraints to check relBaseConstOK = True collisionConstOK = True noConfigJump = True if(start(candidates,numRobots,c,myRmaps,robots,h,env)): # Get their relative Transformation matrix T0_base = [] for myManipulatorIndex in range(len(robots)): T0_base.append(robots[myManipulatorIndex].GetManipulators()[0].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 robot to this path element for myRobotIndex in range(numRobots): currentSphereIndex = candidates[myRobotIndex][c][pElementIndex].sIdx currentTransformIndex = candidates[myRobotIndex][c][pElementIndex].tIdx myRmaps[myRobotIndex].go_to(currentSphereIndex,currentTransformIndex) pathConfigs[myRobotIndex].append(robots[myRobotIndex].GetDOFValues(robots[myRobotIndex].GetManipulators()[0].GetArmIndices())) # DEBUG SECTION FOR SENSING CONFIGURATION JUMP currentConfig[myRobotIndex] = robots[myRobotIndex].GetDOFValues(robots[myRobotIndex].GetManipulators()[0].GetArmIndices()) # print "For robot ",str(myRobotIndex)," ||qA-qB||:" # print "path element ",str(pElementIndex) if(prevConfig[myRobotIndex] != []): # print "previous config: " # print prevConfig[myRobotIndex] # print "current config: " # print currentConfig[myRobotIndex] qdiff = absolute(subtract(currentConfig[myRobotIndex],prevConfig[myRobotIndex])) 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[myRobotIndex] = deepcopy(currentConfig[myRobotIndex]) # Check collision with self and with the environment if(env.CheckCollision(robots[myRobotIndex]) or robots[myRobotIndex].CheckSelfCollision()): collisionConstOK = False return False # If you didn't break yet, wait before the next path element for visualization time.sleep(0.1) # If you made it here, # it means no configuration jump, and no collision return True else: # start() failed return False
from TransformMatrix import * from str2num import * from TSR import * if __name__=='__main__': t="wheel" InitOpenRAVELogging() RaveSetDebugLevel(DebugLevel.Verbose) env = Environment() env.SetViewer('qtcoin') env.Reset() robot = env.ReadRobotXMLFile('../../../openHubo/huboplus/rlhuboplus.robot.xml') env.AddRobot(robot) if(t=="wheel"): target = env.ReadKinBodyXMLFile('../../models/driving_wheel.kinbody.xml') T1 = MakeTransform(rodrigues([0,0,pi/2]),transpose(matrix([0,0,0]))) T2 = MakeTransform(rodrigues([pi/2,0,0]),transpose(matrix([0,0,0]))) T3 = dot(T1,T2) T4 = MakeTransform(eye(3),transpose(matrix([0, 0.8, 1.0]))) T_wheel = dot(T3,T4) target.SetTransform(array(T_wheel)) elif(t=="mug"): target = env.ReadKinBodyXMLFile('../../models/mug1.kinbody.xml') robot.SetDOFValues([-0.7,-0.7],[19,20]) manip = robot.SetActiveManipulator('leftArm') env.AddKinBody(target) sys.stdin.readline() gmodel = grasping.GraspingModel(robot=robot,target=target)
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, '']
self.bConstrainToChain, numTSRs, allTSRstring, self.mimicbodyname) if size(self.mimicbodyjoints) != 0: outstring += ' %d %s ' % (size(self.mimicbodyjoints), Serialize1DIntegerMatrix( self.mimicbodyjoints)) return outstring def SetFirstT0_w(self, T0_w_in): self.TSRs[0].T0_w = copy.deepcopy(T0_w_in) if __name__ == '__main__': juiceTSR = TSR() juiceTSR.Tw_e = MakeTransform(rodrigues([pi / 2, 0, 0]), mat([0, 0.22, 0.1]).T) juiceTSR.Bw = mat([0, 0, 0, 0, -0.02, 0.02, 0, 0, 0, 0, -pi, pi]) juiceTSR.manipindex = 0 juiceTSRChain1 = TSRChain(1, 0) juiceTSRChain1.insertTSR(juiceTSR) juiceTSR.Tw_e = MakeTransform( rodrigues([0, pi, 0]) * rodrigues([pi / 2, 0, 0]), mat([0, 0.22, 0.1]).T) juiceTSRChain2 = TSRChain(1, 0) juiceTSRChain2.insertTSR(juiceTSR) print juiceTSRChain1.Serialize() print juiceTSRChain2.Serialize()
from TSR import * if __name__ == '__main__': t = "wheel" InitOpenRAVELogging() RaveSetDebugLevel(DebugLevel.Verbose) env = Environment() env.SetViewer('qtcoin') env.Reset() robot = env.ReadRobotXMLFile( '../../../openHubo/huboplus/rlhuboplus.robot.xml') env.AddRobot(robot) if (t == "wheel"): target = env.ReadKinBodyXMLFile( '../../models/driving_wheel.kinbody.xml') T1 = MakeTransform(rodrigues([0, 0, pi / 2]), transpose(matrix([0, 0, 0]))) T2 = MakeTransform(rodrigues([pi / 2, 0, 0]), transpose(matrix([0, 0, 0]))) T3 = dot(T1, T2) T4 = MakeTransform(eye(3), transpose(matrix([0, 0.8, 1.0]))) T_wheel = dot(T3, T4) target.SetTransform(array(T_wheel)) elif (t == "mug"): target = env.ReadKinBodyXMLFile('../../models/mug1.kinbody.xml') robot.SetDOFValues([-0.7, -0.7], [19, 20]) manip = robot.SetActiveManipulator('leftArm') env.AddKinBody(target) sys.stdin.readline()
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) if chainlength == 1: #get the TSR's offset frame in w0 coordinates
arm1dofs = [11, 12, 13, 14, 15, 16, 17] activedofs = arm0dofs + arm1dofs initdofvals = r_[3.68, -1.9, -0.0000, 2.2022, -0.0000, 0.0000, 0.0000, 2.6, -1.9, -0.0000, 2.2022, -0.0001, 0.0000, 0.0000] robot.SetActiveDOFs(activedofs) robot.SetActiveDOFValues(initdofvals) # orEnv.LockPhysics(False) time.sleep(0.5) #let the simulator draw the scene # orEnv.LockPhysics(True) #define targets for both hands relative to box in start pose transoffset0 = mat([0.0, -0.285, 0]).T transoffset1 = mat([0.0, 0.305, 0]).T handtrans0 = T0_object[0:3,3] + transoffset0; 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))
robot.SetActiveDOFValues(handdof) time.sleep(0.5) #let the simulator draw the scene #set the active dof robot.SetActiveDOFs(activedofs) #let's define two TSR chains for this task, they differ only in the rotation of the hand #first TSR chain #place the first TSR's reference frame at the object's frame relative to world frame T0_w = T0_object #get the TSR's offset frame in w coordinates Tw_e1 = MakeTransform(rodrigues([pi / 2, 0, 0]), mat([0, 0.20, 0.1]).T) #define bounds to only allow rotation of the hand about z axis and a small deviation in translation along the z axis Bw = mat([0, 0, 0, 0, -0.02, 0.02, 0, 0, 0, 0, -pi, pi]) TSRstring1 = SerializeTSR(0, 'NULL', T0_w, Tw_e1, Bw) TSRChainString1 = SerializeTSRChain(0, 1, 0, 1, TSRstring1, 'NULL', []) #now define the second TSR chain #it is the same as the first TSR Chain except Tw_e is different (the hand is rotated by 180 degrees about its z axis) Tw_e2 = MakeTransform( rodrigues([0, pi, 0]) * rodrigues([pi / 2, 0, 0]), mat([0, 0.20, 0.1]).T) TSRstring2 = SerializeTSR(0, 'NULL', T0_w, Tw_e2, Bw) TSRChainString2 = SerializeTSRChain(0, 1, 0, 1, TSRstring2, 'NULL', [])
rlhuboplus, rlhuboplus.GetManipulators()[0]) rlhuboplusRightRm = ReachabilityMap("./rlhuboplus_rightArm_ik_solver", rlhuboplus, rlhuboplus.GetManipulators()[1]) print "loading" rlhuboplusLeftRm.load("rlhuboplus_left_m12") rlhuboplusRightRm.load("rlhuboplus_right_m12") env.SetViewer('qtcoin') h = misc.DrawAxes( env, array( MakeTransform(matrix(rodrigues([0, 0, 0])), transpose(matrix([0.0, 0.0, 0.0])))), 1.0) rlhuboplusLeftRm.map[1025].show(env, array((0, 0, 1, 0.5))) rlhuboplusLeftRm.map[1026].show(env, array((0, 0, 1, 0.5))) rlhuboplusLeftRm.map[1055].show(env, array((0, 0, 1, 0.5))) rlhuboplusLeftRm.map[1056].show(env, array((0, 0, 1, 0.5))) rlhuboplusLeftRm.map[1227].show(env, array((0, 0, 1, 0.5))) rlhuboplusLeftRm.map[1228].show(env, array((0, 0, 1, 0.5))) rlhuboplusLeftRm.hide() rlhuboplusRightRm.map[1071].show(env, array((1, 0, 0, 0.5))) rlhuboplusRightRm.map[1070].show(env, array((1, 0, 0, 0.5))) rlhuboplusRightRm.map[1101].show(env, array((1, 0, 0, 0.5))) rlhuboplusRightRm.map[1100].show(env, array((1, 0, 0, 0.5))) rlhuboplusRightRm.map[1257].show(env, array((1, 0, 0, 0.5)))
def get_pairs(TLH_RH, env, robot, myRmaps): # h.append(misc.DrawAxes(env,array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.0])))),1.0)) gR = 0.0 # ground color gG = 0.0 # ground color gB = 0.0 # ground color ge = 5.0 # ground extension h.append( env.drawtrimesh(points=array(((0, 0, 0), (ge, 0, 0), (0, ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array(((ge, 0, 0), (0, ge, 0), (ge, ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array(((0, 0, 0), (-ge, 0, 0), (0, -ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array( ((-ge, 0, 0), (0, -ge, 0), (-ge, -ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array(((0, 0, 0), (-ge, 0, 0), (0, ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array(((-ge, 0, 0), (0, ge, 0), (-ge, ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array(((0, 0, 0), (ge, 0, 0), (0, -ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array(((ge, 0, 0), (0, -ge, 0), (ge, -ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) # Define robot base constraint(s) # a) Bounds <type 'list'> # xyz in meters, rpy in radians # [1.0, 1.0, 1.0, 0.0, 0.0, 0.0] would mean, we allow robot bases # to be 1 meter apart in each direction but we want them to have the # same rotation # Relative Base Constraints between the two robots #relBaseConstraint = MakeTransform() # This is type (b) relBaseConstraint = MakeTransform(matrix(rodrigues([0, 0, 0])), transpose(matrix([0.0, 0.0, 0.0]))) # Base Constraints of the first (master) robot in the world coordinate frame # Same as relative base constraints. First 3 elements are XYZ bounds and the last three elements determine the desired rotation matrix of the base of the master robot in world coords. masterBaseConstraint = [1.0, 1.0, 0.0, 0.0, 0.0, 0.0] # b) Exact transform <type 'numpy.ndarray'> # relBaseConstraint = dot(something, some_other_thing) # Try to find a valid candidate that satisfies # all the constraints we have (base location, collision, and configuration-jump) print "Ready to find sisters... ", str(datetime.now()) resp = find_sister_pairs(myRmaps, [relBaseConstraint], [TLH_RH], env) # resp can be None or [pairs, rm, p] if resp == None: return resp else: return [resp[0], resp[1], [relBaseConstraint]]
if __name__ == '__main__': env = Environment() env.SetViewer('qtcoin') # Add huboplus robot = env.ReadRobotURI('../../../openHubo/huboplus/huboplus.robot.xml') env.Add(robot) lowerLimits, upperLimits = robot.GetDOFLimits() # Let's keep the rotation of the robot around it's Z-Axis in a variable... rotz = [] rotz.append(pi / 3) # Define the transform and apply the transform shift_robot0 = MakeTransform(matrix(rodrigues([0, 0, rotz[0]])), transpose(matrix([-2.5, 0.0, 0.95]))) robot.SetTransform(array(shift_robot0)) probs_cbirrt = RaveCreateModule(env, 'CBiRRT') try: env.AddModule(probs_cbirrt, robot.GetName( )) # this string should match to <Robot name="" > in robot.xml except openrave_exception, e: print e print "Getting Loaded Problems" probs = env.GetLoadedProblems() # Load the reachability map for left arm
occupancy = {} reachability_dict = {} env = Environment() RaveSetDebugLevel(1) robot = env.ReadRobotURI( '../../../openHubo/drchubo/drchubo-urdf/robots/drchubo.robot.xml') env.Add(robot) manips = robot.GetManipulators() drchubo_rightFoot = manips[3] rf_eet = drchubo_rightFoot.GetEndEffectorTransform( ) # right foot end effector transform shift_drchubo_to_its_right_foot = MakeTransform( matrix(rodrigues([0, 0, 0])), transpose(matrix([0.5, -1 * rf_eet[1][3], -1 * rf_eet[2][3]]))) robot.SetTransform(array(shift_drchubo_to_its_right_foot)) # box 0 mybox0 = RaveCreateKinBody(env, '') mybox0.SetName('box_0') mybox0.InitFromBoxes(numpy.array([[0.8, 0.45, 0.74, 0.025, 0.025, 0.3]]), True) # set geometry as one box of extents 0.1, 0.2, 0.3 env.Add(mybox0, True) # box 1 mybox1 = RaveCreateKinBody(env, '') mybox1.SetName('box_1') mybox1.InitFromBoxes(numpy.array([[0.7, -0.27, 1.3, 0.05, 0.05, 0.5]]), True) # set geometry as one box of extents 0.1, 0.2, 0.3
## Constraint Based Manipulation ## from rodrigues import * from TransformMatrix import * from str2num import * from TSR import * # robot-placement common functions from roplace_common import * # This changes the height and the pitch angle of the wheel version = 1 env = Environment() env.SetViewer('qtcoin') T0_p = MakeTransform(matrix(rodrigues([0, 0, 0])), transpose(matrix([0.0, 0.0, 0.0]))) # 1. Create a trajectory for the tool center point to follow # Left Hand traj0 = [] Tstart0 = array( MakeTransform(matrix(rodrigues([0, 0, 0])), transpose(matrix([0.0, 0.0, 0.0])))) traj0.append(Tstart0) # traj0.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.05]))))) #traj0.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.05]))))) #traj0.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.1])))))
def get_pairs(pitch, height, dist, env, robot, myRmaps): h.append( misc.DrawAxes( env, array( MakeTransform(matrix(rodrigues([0, 0, 0])), transpose(matrix([0.0, 0.0, 0.0])))), 1.0)) gR = 0.6 # ground color gG = 0.6 # ground color gB = 0.6 # ground color ge = 5.0 # ground extension h.append( env.drawtrimesh(points=array(((0, 0, 0), (ge, 0, 0), (0, ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array(((ge, 0, 0), (0, ge, 0), (ge, ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array(((0, 0, 0), (-ge, 0, 0), (0, -ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array( ((-ge, 0, 0), (0, -ge, 0), (-ge, -ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array(((0, 0, 0), (-ge, 0, 0), (0, ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array(((-ge, 0, 0), (0, ge, 0), (-ge, ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array(((0, 0, 0), (ge, 0, 0), (0, -ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) h.append( env.drawtrimesh(points=array(((ge, 0, 0), (0, -ge, 0), (ge, -ge, 0))), indices=None, colors=array( ((gR, gG, gB), (gR, gG, gB), (gR, gG, gB))))) Tee = [] manips = robot.GetManipulators() for i in range(len(manips)): # Returns End Effector Transform in World Coordinates Tlink = manips[i].GetEndEffectorTransform() Tee.append(Tlink) # Where do we want the end effectors (hands) to start from in world coordinates? T0_LIFT = MakeTransform(matrix(rodrigues([0, pitch, 0])), transpose(matrix([0.0, 0.0, height]))) TLIFT_LH = MakeTransform(matrix(rodrigues([0, 0, 0])), transpose(matrix([0.0, 0.0, 0.0]))) T0_LH = dot(T0_LIFT, TLIFT_LH) h.append(misc.DrawAxes(env, T0_LH, 0.4)) TLIFT_RH = MakeTransform(matrix(rodrigues([0, 0, 0])), transpose(matrix([0.0, -dist, 0.0]))) T0_RH = dot(T0_LIFT, TLIFT_RH) h.append(misc.DrawAxes(env, T0_RH, 0.4)) # Define robot base constraint(s) # a) Bounds <type 'list'> # xyz in meters, rpy in radians # [1.0, 1.0, 1.0, 0.0, 0.0, 0.0] would mean, we allow robot bases # to be 1 meter apart in each direction but we want them to have the # same rotation # Relative Base Constraints between the two robots #relBaseConstraint = MakeTransform() # This is type (b) relBaseConstraint = MakeTransform(matrix(rodrigues([0, 0, 0])), transpose(matrix([0.0, 0.0, 0.0]))) # Base Constraints of the first (master) robot in the world coordinate frame # Same as relative base constraints. First 3 elements are XYZ bounds and the last three elements determine the desired rotation matrix of the base of the master robot in world coords. masterBaseConstraint = [1.0, 1.0, 0.0, 0.0, 0.0, 0.0] # b) Exact transform <type 'numpy.ndarray'> # relBaseConstraint = dot(something, some_other_thing) # Try to find a valid candidate that satisfies # all the constraints we have (base location, collision, and configuration-jump) # Relative transform of initial grasp transforms TLH_RH = dot(linalg.inv(T0_LH), T0_RH) print "Ready to find sisters... ", str(datetime.now()) resp = find_sister_pairs(myRmaps, [relBaseConstraint], [TLH_RH], env) # resp can be None or [pairs, rm, p] if resp == None: return resp else: return [resp[0], resp[1], [relBaseConstraint], [TLH_RH], T0_LH, T0_RH]
# #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 return masterBaseConstOK env = Environment() env.SetViewer('qtcoin') T0_p = MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.0]))) # 1. Create a trajectory for the tool center point to follow Tstart = array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.0])))) Tgoal = array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,-0.2,0.0])))) traj = [] traj.append(Tstart) traj.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,-0.02,0.0]))))) traj.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,-0.07,0.0]))))) traj.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,-0.12,0.0]))))) traj.append(array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,-0.16,0.0]))))) traj.append(Tgoal) # Just to try # Move, then rotate around the axis with that shift being the radius
handles = [] normalsmoothingitrs = 150; fastsmoothingitrs = 20; env = Environment() RaveSetDebugLevel(DebugLevel.Info) # set output level to debug robotid = env.ReadRobotURI('../../../openHubo/drchubo/robots/drchubo.robot.xml') crankid = env.ReadRobotURI('../../../../drc_common/models/driving_wheel.robot.xml') env.Add(robotid) env.Add(crankid) env.SetViewer('qtcoin') shiftUp = 0.95 # Move the wheel infront of the robot 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(env,'CBiRRT') probs_crankmover = RaveCreateModule(env,'CBiRRT') manips = robotid.GetManipulators() crankmanip = crankid.GetManipulators() try: env.AddModule(probs_cbirrt,robotid.GetName()) env.AddModule(probs_crankmover,crankid.GetName()) except openrave_exception, e: print e print "Getting Loaded Problems" probs = env.GetLoadedProblems()
env.Add(robots[len(robots)-1]) # add the last robot in the environment so that we can rename it. for body in env.GetBodies(): rname = body.GetName() if(rname == 'BarrettWAM'): newname = 'robot'+str(i) body.SetName(newname) print body # drchubo_manips = drchubo.GetManipulators() # drchubo_rightFoot = drchubo_manips[3] # eet1 = robot1.GetEndEffectorTransform() # right foot end effector transform shift_robot0 = MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.0]))) shift_robot1 = MakeTransform(matrix(rodrigues([0,0,pi])),transpose(matrix([-1.0,0.0,0.0]))) robots[0].SetTransform(array(shift_robot0)) robots[1].SetTransform(array(shift_robot1)) mybox = RaveCreateKinBody(env,'') mybox.SetName('box') mybox.InitFromBoxes(numpy.array([[-0.5,0,0.3,0.025,0.025,0.3]]),True) # set geometry as one box of extents 0.1, 0.2, 0.3 env.Add(mybox,True) h=misc.DrawAxes(env,mybox.GetTransform(),1.0) print "Done! Press Enter to exit..." sys.stdin.readline()
return self.solved() def continousSolve(self,itrs=1000,auto=False,extra=[]): #Enable TSR sampling self.sample_bw=True for k in range(itrs): if not(self.robot.GetEnv().CheckCollision(self.robot)) and not(self.robot.CheckSelfCollision()) and self.solved(): print "Valid solution found!" #TODO: log solutuon + affine DOF #rezero to prevent getting stuck...at major speed penalty self.robot.SetDOFValues(self.zero) self.run(auto,extra) time.sleep(.1) return self.solved() def resetZero(self): self.zero=self.robot.GetDOFValues() if __name__ == '__main__': juiceTSR = TSR() juiceTSR.Tw_e = MakeTransform(rodrigues([pi/2, 0, 0]),mat([0, 0.22, 0.1]).T) juiceTSR.Bw = mat([0, 0, 0, 0, -0.02, 0.02, 0, 0, 0, 0, -pi, pi]) juiceTSR.manipindex = 0 test=GeneralIK('','',[juiceTSR],False) test.supportlinks=['leftFootBase'] test.cogtarget=(.1,.2,.3) print test.Serialize()
def trans_to_str(T): myStr = "" for c in range(0,3): for r in range(0,3): myStr += str(T[r,c])+" " for r in range(0,3): myStr += str(T[r,3])+" " return myStr env = Environment() env.SetViewer('qtcoin') h = [] h.append(misc.DrawAxes(env,array(MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([0.0,0.0,0.0])))),1.0)) # Add drchubo robots = [] # limitless drchubo (old) robots.append(env.ReadRobotURI('../../../openHubo/drchubo/old/drchubo-urdf/robots/drchubo2.robot.xml')) # up-to-date drchubo with joint limits defined robots.append(env.ReadRobotURI('../../../openHubo/drchubo/robots/drchubo2.robot.xml')) # Which robot? idx = 0 env.Add(robots[idx])
fastsmoothingitrs = 20 env = Environment() RaveSetDebugLevel(DebugLevel.Info) # set output level to debug robotid = env.ReadRobotURI( '../../../openHubo/huboplus/rlhuboplus_mit.robot.xml') crankid = env.ReadRobotURI( '../../../../drc_common/models/driving_wheel.robot.xml') env.Add(robotid) env.Add(crankid) env.SetViewer('qtcoin') # Move the wheel infront of the robot crankid.SetTransform( array( MakeTransform( dot(rodrigues([0, 0, pi / 2]), rodrigues([pi / 2, 0, 0])), transpose(matrix([0.18, 0.09, 0.9]))))) probs_cbirrt = RaveCreateModule(env, 'CBiRRT') probs_crankmover = RaveCreateModule(env, 'CBiRRT') manips = robotid.GetManipulators() crankmanip = crankid.GetManipulators() try: env.AddModule( probs_cbirrt, 'rlhuboplus') # this string should match to kinematic body env.AddModule(probs_crankmover, 'crank') except openrave_exception, e: print e
myDisc.InitFromGeometries([infocylinder]) # we could add more cylinders here myDisc.SetName('valve') env.Add(myDisc,True) env.Add(robotid) env.Add(crankid) env.SetViewer('qtcoin') shiftUp = 1.25 worldPitch = 0.0 tiltDiff = acos(dot(linalg.inv(crankid.GetManipulators()[0].GetEndEffectorTransform()),crankid.GetLinks()[0].GetTransform())[1,1]) # Move the wheel infront of the robot crankid.SetTransform(array(MakeTransform(dot(rodrigues([0,0,pi/2]),rodrigues([(pi/2-tiltDiff+worldPitch),0,0])),transpose(matrix([0.6, 0.0, shiftUp]))))) myDisc.SetTransform(crankid.GetManipulators()[0].GetTransform()) # sys.stdin.readline() probs_cbirrt = RaveCreateModule(env,'CBiRRT') probs_crankmover = RaveCreateModule(env,'CBiRRT') manips = robotid.GetManipulators() crankmanip = crankid.GetManipulators() try: env.AddModule(probs_cbirrt,robotid.GetName()) env.AddModule(probs_crankmover,crankid.GetName()) except openrave_exception, e: print e
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 generate_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() # printself.probs[0] # --> self.probs_cbirrt # printself.probs[1] # --> self.probs_crankmover self.robot.SetActiveDOFs([15, 16, 17, 18, 19, 20, 21, 27, 28, 29, 30, 31, 32, 33]) self.initconfig = self.robot.GetActiveDOFValues() # print"initconfig" # printself.initconfig # printtype(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, -pi / 8, 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([pi / 4, 0, 0])), transpose(matrix([0.1, 0.1, -0.2]))) ) self.T0_LH1_APPROACH_2 = dot( temp, MakeTransform(matrix(rodrigues([pi / 4, 0, 0])), transpose(matrix([0.03, 0.0, -0.15]))) ) self.T0_LH1_ENTRY = dot( temp, MakeTransform(matrix(rodrigues([pi / 4, 0, 0])), transpose(matrix([-0.01, 0.01, -0.14]))) ) # 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 / 8, 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([-pi / 4, 0, 0])), transpose(matrix([0.1, -0.1, -0.2]))) ) self.T0_RH1_APPROACH_2 = dot( temp, MakeTransform(matrix(rodrigues([-pi / 4, 0, 0])), transpose(matrix([0.03, 0.0, -0.15]))) ) self.T0_RH1_ENTRY = dot( temp, MakeTransform(matrix(rodrigues([-pi / 4, 0, 0])), transpose(matrix([-0.01, -0.01, -0.12]))) ) ################################################################ ## 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", " ") #################################################################### ## 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 not self.is_valid(self.startik, "startik"): # print"Hey: No IK Solution found! Move the robot!" # 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 / 4 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 not self.is_valid(self.goalik, "goalik"): # print"Hey: No IK Solution found! Move the robot!" # self.notPlanning = True return [] self.crankid.SetDOFValues([crank_rot], [self.crankjointind]) ######################################################### # This is the end of the second block in Matlab version # ######################################################### self.crankid.SetDOFValues([self.crankjointind]) # self.notPlanning = True return 1
for x in range(21): # x is on the X-Axis of roboground and facing wherever we want it to TCOM_LF = dot(linalg.inv(T0_COM), T0_LF) TCOM_RF = dot(linalg.inv(T0_COM), T0_RF) #Let's shift roboground to where the center of mass is on the ground # Note TCOMXY is where the COM is but it's rotation is the same with the world # TODO: Instead of calculating T0_COMXY's rotation from T0_FACING, # get the torso frame and rotate it back to being flat # around x and y but don't touch the orientation around z. T0_COMXY = array( MakeTransform(T0_FACING[0:3, 0:3], transpose(matrix([T0_COM[0, 3], T0_COM[1, 3], 0])))) #TCOMXY_LFTARGET = array(MakeTransform(rodrigues([0,0,0]),transpose(matrix([((-0.1)+x*0.01),TCOM_LF[1,3],0.0])))) TCOMXY_LFTARGET = array( MakeTransform(rodrigues([0, 0, 0]), transpose(matrix([((-0.1) + x * 0.01), 0.09, 0.0])))) #TCOMXY_RFTARGET = array(MakeTransform(rodrigues([0,0,0]),transpose(matrix([((-0.1)+x*0.01),TCOM_RF[1,3],0.0])))) TCOMXY_RFTARGET = array( MakeTransform(rodrigues([0, 0, 0]), transpose(matrix([((-0.1) + x * 0.01), -0.09, 0.0])))) # Now we know where to face and where the feet should be T0_LFTARGET = dot(T0_COMXY, TCOMXY_LFTARGET) # T0_LFTARGET = dot(T0_TORSOXY,array(MakeTransform(rodrigues([0,0,0]),transpose(matrix([((-0.1)+x*0.01),0.1,0.0]))))) T0_RFTARGET = dot(T0_COMXY, TCOMXY_RFTARGET) # T0_RFTARGET = dot(T0_TORSOXY,array(MakeTransform(rodrigues([0,0,0]),transpose(matrix([((-0.1)+x*0.01),-0.1,0.0]))))) # old code - delete if the code above works #
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
#set printing, display options, and collision checker orEnv.SetDebugLevel(DebugLevel.Info) colchecker = RaveCreateCollisionChecker(orEnv, 'ode') orEnv.SetCollisionChecker(colchecker) #create problem instances probs_cbirrt = RaveCreateProblem(orEnv, 'SOCBiRRT') orEnv.LoadProblem(probs_cbirrt, 'BarrettWAM') #TSR chain #place the first TSR's reference frame at the object's frame relative to world frame T0_w = T0_object #get the TSR's offset frame in w coordinates Tw_e1 = MakeTransform(rodrigues([0, 0, 0]), mat([0, 0.0, -0.05]).T) #define bounds to only allow rotation of the hand about z axis and a small deviation in translation along the z axis Bw = mat([0, 0, 0, 0, -0.02, 0.02, -pi / 2, pi / 2, -pi / 2, pi / 2, 0, 0]) TSRstring1 = SerializeTSR(0, 'NULL', T0_w, Tw_e1, Bw) TSRChainString1 = SerializeTSRChain(0, 1, 0, 1, TSRstring1, 'NULL', []) cmd = 'RunSoCBiRRT ' cmd = cmd + 'timelimit %d ' % (20) cmd = cmd + 'goalobject %s ' % ('juice') cmd = cmd + 'goalvelocitymagnitude %f ' % (0.0025) cmd = cmd + 'thickness %f ' % (0.25) cmd = cmd + 'screenshot %d ' % (0) cmd = cmd + 'mergetree %d ' % (1) cmd = cmd + 'planning_theta %d ' % (4) #cmd = cmd + '1.836 100.641 0.032 0.085 '
self.TSRs[0].T0_w = copy.deepcopy(T0_w_in) def SetFirstTw_e(self,Tw_e_in): self.TSRs[0].Tw_e = copy.deepcopy(Tw_e_in) def GetFirstTw_e(self): return self.TSRs[0].Tw_e def SetManipIndex(self, manipindex_in): for i in range(0,len(self.TSRs)): self.TSRs[i].SetManipIndex(manipindex_in) if __name__ == '__main__': juiceTSR = TSR() juiceTSR.Tw_e = MakeTransform(rodrigues([pi/2, 0, 0]),mat([0, 0.22, 0.1]).T) juiceTSR.Bw = mat([0, 0, 0, 0, -0.02, 0.02, 0, 0, 0, 0, -pi, pi]) juiceTSR.manipindex = 0 juiceTSRChain1 = TSRChain(1,0) juiceTSRChain1.insertTSR(juiceTSR) juiceTSR.Tw_e = MakeTransform(rodrigues([0, pi, 0])*rodrigues([pi/2, 0, 0]),mat([0, 0.22, 0.1]).T) juiceTSRChain2 = TSRChain(1,0) juiceTSRChain2.insertTSR(juiceTSR) print juiceTSRChain1.Serialize() print juiceTSRChain2.Serialize()
robots = [] # the following for loop will add N robots from the same xml file # and rename them to prevent failure for i in range(2): robots.append(env.ReadRobotURI('robots/barrettwam.robot.xml')) env.Add(robots[len(robots)-1]) # add the last robot in the environment so that we can rename it. for body in env.GetBodies(): rname = body.GetName() if(rname == 'BarrettWAM'): newname = 'BarrettWAM_'+str(i) body.SetName(newname) print body shift_robot0 = MakeTransform(matrix(rodrigues([0,0,pi])),transpose(matrix([0.5,0.0,0.0]))) shift_robot1 = MakeTransform(matrix(rodrigues([0,0,0])),transpose(matrix([-1.5,0.0,0.0]))) robots[0].SetTransform(array(shift_robot0)) robots[1].SetTransform(array(shift_robot1)) mybox = RaveCreateKinBody(env,'') mybox.SetName('box') mybox.InitFromBoxes(numpy.array([[-0.5,0,0.3,0.025,0.025,0.3]]),True) # set geometry as one box of extents 0.1, 0.2, 0.3 env.Add(mybox,True) handles = [] handles.append(misc.DrawAxes(env,mybox.GetTransform(),1.0)) gmodels = []