Esempio n. 1
0
 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'
     ])
Esempio n. 2
0
 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)
Esempio n. 3
0
 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)
Esempio n. 4
0
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
Esempio n. 5
0
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
Esempio n. 6
0
def getRollPitchYawFromRobotState(robotState):
    return transformUtils.quaternionToRollPitchYaw(robotState[3:7])
Esempio n. 7
0
 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'])
Esempio n. 8
0
def getRollPitchYawFromRobotState(robotState):
    return transformUtils.quaternionToRollPitchYaw(robotState[3:7])