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 = botpy.quat_to_roll_pitch_yaw(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 = botpy.quat_to_roll_pitch_yaw(quat)
pose = np.hstack((trans, rpy, jointPositions))
assert len(pose) == getNumPositions()
return pose
def getRollPitchYawFromRobotState(robotState):
return botpy.quat_to_roll_pitch_yaw(robotState[3:7])
def getRollPitchYawFromRobotState(robotState):
return botpy.quat_to_roll_pitch_yaw(robotState[3:7])
