def moveHandModelToFrame(model, frame):
pos, quat = transformUtils.poseFromTransform(frame)
rpy = transformUtils.quaternionToRollPitchYaw(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 = transformUtils.quaternionToRollPitchYaw(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 = transformUtils.quaternionToRollPitchYaw(msg.orientation)
pose = self.jointController.q.copy()
pose[0:3] = msg.pos
pose[3:6] = rpy
self.bdiJointController.setPose("ERS BDI", pose)
def convertStateMessageToDrakePose(msg):
jointMap = {}
for name, position in zip(msg.joint_name, msg.joint_position):
jointMap[name] = position
jointPositions = []
for name in getDrakePoseJointNames()[6:]:
jointPositions.append(jointMap[name])
trans = msg.pose.translation
quat = msg.pose.rotation
trans = [trans.x, trans.y, trans.z]
quat = [quat.w, quat.x, quat.y, quat.z]
rpy = transformUtils.quaternionToRollPitchYaw(quat)
pose = np.hstack((trans, rpy, jointPositions))
assert len(pose) == getNumPositions()
return pose
def convertStateMessageToDrakePose(msg):
jointMap = {}
for name, position in zip(msg.joint_name, msg.joint_position):
jointMap[name] = position
jointPositions = []
for name in getDrakePoseJointNames()[6:]:
jointPositions.append(jointMap[name])
trans = msg.pose.translation
quat = msg.pose.rotation
trans = [trans.x, trans.y, trans.z]
quat = [quat.w, quat.x, quat.y, quat.z]
rpy = transformUtils.quaternionToRollPitchYaw(quat)
pose = np.hstack((trans, rpy, jointPositions))
assert len(pose) == getNumPositions()
return pose
def getRollPitchYawFromRobotState(robotState):
return transformUtils.quaternionToRollPitchYaw(robotState[3:7])
def moveHandModelToFrame(model, frame):
pos, quat = transformUtils.poseFromTransform(frame)
rpy = transformUtils.quaternionToRollPitchYaw(quat)
pose = np.hstack((pos, rpy))
model.model.setJointPositions(pose, ['base_x', 'base_y', 'base_z', 'base_roll', 'base_pitch', 'base_yaw'])
def getRollPitchYawFromRobotState(robotState):
return transformUtils.quaternionToRollPitchYaw(robotState[3:7])
