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 handle_task_control(self,req): print "Playing trajectories" ####################################################### ## WE'RE DONE GENERATING TRAJECTORIES, NOW PLAY THEM ## ####################################################### pr2_rave2task_control() # This function packs movetraj files in 3 packages and sends them over to task_control. ########################################## ## DONE PLAYING THE TRAJECTORIES - EXIT ## ########################################## print "press enter to exit" sys.stdin.readline() emptyResponse = [] return emptyResponse
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