def moveHandModelToFrame(model, frame):
pos, quat = transformUtils.poseFromTransform(frame)
rpy = botpy.quat_to_roll_pitch_yaw(quat)
pose = np.hstack((pos, rpy))
model.model.setJointPositions(pose, [
'base_x', 'base_y', 'base_z', 'base_roll', 'base_pitch', 'base_yaw'
])
def onPoseBDI(self,msg):
self.pose_bdi = msg
# Set the xyzrpy of this pose to equal that estimated by BDI
rpy = botpy.quat_to_roll_pitch_yaw(msg.orientation)
pose = self.jointController.q.copy()
pose[0:3] = msg.pos
pose[3:6] = rpy
self.bdiJointController.setPose("ERS BDI", pose)
def onPoseBDI(self,msg):
self.pose_bdi = msg
# Set the xyzrpy of this pose to equal that estimated by BDI
rpy = botpy.quat_to_roll_pitch_yaw(msg.orientation)
pose = self.jointController.q.copy()
pose[0:3] = msg.pos
pose[3:6] = rpy
self.bdiJointController.setPose("ERS BDI", pose)
def TargetReceived(self, data):
self.log( 'Target received (%.3f,%.3f,%.3f), (%.3f,%.3f,%.3f,%.3f)' % \
(data.trans[0], data.trans[1], data.trans[2], \
data.quat[0], data.quat[1], data.quat[2], data.quat[3]) )
if math.isnan(data.trans[0]):
self.log('Getting NaN target, stop')
return
self.TargetMsg = deepcopy(data)
targetToWorld = transformUtils.frameFromPositionAndRPY(self.TargetMsg.trans,
np.degrees(botpy.quat_to_roll_pitch_yaw(self.TargetMsg.quat)))
startPose = self.getPlanningStartPose()
handToWorld= self.ikPlanner.getLinkFrameAtPose( 'l_hand_face', startPose)
goalFrame = vis.updateFrame(handToWorld, 'OriginalFrame', parent='Pfgrasp', visible=True, scale=0.25)
goalFrame2 = vis.updateFrame(targetToWorld, 'PeterFrame', parent='Pfgrasp', visible=True, scale=0.25)
handToWorld_XYZ = handToWorld.GetPosition()
targetToWorld_XYZ = targetToWorld.GetPosition()
dist = sqrt( (handToWorld_XYZ[0]-targetToWorld_XYZ[0])**2 + (handToWorld_XYZ[1]-targetToWorld_XYZ[1])**2 + (handToWorld_XYZ[2]-targetToWorld_XYZ[2])**2 )
self.log( "dist %.3f" % dist )
threshold = float(self.ui.criterionEdit.text)
if(dist < threshold):
#easygui.msgbox("The correction movement is less than 0.015, you can go grasp it", title="Done")
self.log("The correction movement is %.3f less than %.3f, you can go grasp it" % (dist, threshold))
if self.autoMode: self.guardedMoveForwardAndGraspHoldRetreat()
else:
#print "startPose", startPose
#print "targetToWorld", targetToWorld
#print "graspingHand", self.graspingHand
constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, self.graspingHand, targetToWorld, lockBase=False, lockBack=True)
endPose, info = constraintSet.runIk()
if info > 10:
self.log("in Target received: Bad movement")
return
graspPlan = constraintSet.runIkTraj()
if self.autoMode:
self.manipPlanner.commitManipPlan(graspPlan)
self.waitForPlanExecution(graspPlan)
self.runoneiter()
def testEulerBotpy():
'''
Test some quaternion and euler conversions with botpy
'''
quat = transformations.random_quaternion()
rpy = transformations.euler_from_quaternion(quat)
rpy2 = botpy.quat_to_roll_pitch_yaw(quat)
quat2 = botpy.roll_pitch_yaw_to_quat(rpy)
mat = transformations.quaternion_matrix(quat)
frame = transformUtils.getTransformFromNumpy(mat)
rpy3 = transformUtils.rollPitchYawFromTransform(frame)
print quat, quat2
print rpy, rpy2, rpy3
assert isQuatEqual(quat, quat2)
assert np.allclose(rpy, rpy2)
assert np.allclose(rpy2, rpy3)
def moveHandModelToFrame(model, frame):
pos, quat = transformUtils.poseFromTransform(frame)
rpy = botpy.quat_to_roll_pitch_yaw(quat)
pose = np.hstack((pos, rpy))
model.model.setJointPositions(pose, ["base_x", "base_y", "base_z", "base_roll", "base_pitch", "base_yaw"])
def quaternionToRollPitchYaw(quat):
return botpy.quat_to_roll_pitch_yaw(quat)
