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
