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 convertStateMessageToDrakePose(msg, strict=True): ''' If strict is true, then the state message must contain a joint_position for each named joint in the drake pose joint names. If strict is false, then a default value of 0.0 is used to fill joint positions that are not specified in the robot state msg argument. ''' jointMap = {} for name, position in zip(msg.joint_name, msg.joint_position): jointMap[name] = position jointPositions = [] for name in getDrakePoseJointNames()[6:]: if strict: jointPositions.append(jointMap[name]) else: jointPositions.append(jointMap.get(name, 0.0)) 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 generateBoxLinkNode(self, aff): link = etree.Element("link", {"name": aff.getDescription()["Name"]}) link.append(etree.Element("pose")) pose = np.append( aff.getDescription()["pose"][0], transformUtils.quaternionToRollPitchYaw( aff.getDescription()["pose"][1]), ) link.find("pose").text = " ".join(map(str, pose)) dimensions = aff.getDescription()["Dimensions"] if aff.getDescription()["Collision Enabled"]: link.append(etree.Element("collision", {"name": "collision"})) link.find("collision").append(etree.Element("geometry")) link.find("collision/geometry").append(etree.Element("box")) link.find("collision/geometry/box").append(etree.Element("size")) link.find("collision/geometry/box/size").text = " ".join( map(str, dimensions)) link.append(etree.Element("visual", {"name": "visual"})) link.find("visual").append(etree.Element("geometry")) link.find("visual/geometry").append(etree.Element("box")) link.find("visual/geometry/box").append(etree.Element("size")) link.find("visual/geometry/box/size").text = " ".join( map(str, dimensions)) link.find("visual").append(etree.Element("material")) return link
def convertStateMessageToDrakePoseBasic(self, trans, quat, jointNamesIn, jointPositionsIn, strict=True): """ If strict is true, then the state message must contain a joint_position for each named joint in the drake pose joint names. If strict is false, then a default value of 0.0 is used to fill joint positions that are not specified in the robot state msg argument. This version is used for ROS messages """ jointMap = {} for name, position in zip(jointNamesIn, jointPositionsIn): jointMap[name] = position jointPositions = [] for name in self.getStateJointNames(): if strict: jointPositions.append(jointMap[name]) else: jointPositions.append(jointMap.get(name, 0.0)) rpy = transformUtils.quaternionToRollPitchYaw(quat) pose = np.hstack((trans, rpy, jointPositions)) assert len(pose) == self.getStateLength() return pose
def generateBoxLinkNode(self, aff): link = etree.Element('link', {'name': aff.getDescription()['Name']}) link.append(etree.Element('pose')) pose = np.append( aff.getDescription()['pose'][0], transformUtils.quaternionToRollPitchYaw( aff.getDescription()['pose'][1])) link.find('pose').text = ' '.join(map(str, pose)) dimensions = aff.getDescription()['Dimensions'] if aff.getDescription()['Collision Enabled']: link.append(etree.Element('collision', {'name': 'collision'})) link.find('collision').append(etree.Element('geometry')) link.find('collision/geometry').append(etree.Element('box')) link.find('collision/geometry/box').append(etree.Element('size')) link.find('collision/geometry/box/size').text = ' '.join( map(str, dimensions)) link.append(etree.Element('visual', {'name': 'visual'})) link.find('visual').append(etree.Element('geometry')) link.find('visual/geometry').append(etree.Element('box')) link.find('visual/geometry/box').append(etree.Element('size')) link.find('visual/geometry/box/size').text = ' '.join( map(str, dimensions)) link.find('visual').append(etree.Element('material')) link.find('visual/material').append(etree.Element('diffuse')) link.find('visual/material/diffuse').text = '{:s} {:.1f}'.format( ' '.join(map(str, aff.getDescription()['Color'])), aff.getDescription()['Alpha']) link.find('visual/material').append(etree.Element('ambient')) link.find('visual/material/ambient').text = '0 0 0 1' return link
def generateCylinderLinkNode(self, aff): link = etree.Element('link', {'name':aff.getDescription()['Name']}) link.append(etree.Element('pose')) pose = np.append(aff.getDescription()['pose'][0], transformUtils.quaternionToRollPitchYaw(aff.getDescription()['pose'][1])) link.find('pose').text = ' '.join(map(str, pose)) if aff.getDescription()['Collision Enabled']: link.append(etree.Element('collision', {'name': 'collision'})) link.find('collision').append(etree.Element('geometry')) link.find('collision/geometry').append(etree.Element('cylinder')) link.find('collision/geometry/cylinder').append(etree.Element('radius')) link.find('collision/geometry/cylinder/radius').text = '{:.4f}'.format(aff.getDescription()['Radius']) link.find('collision/geometry/cylinder').append(etree.Element('length')) link.find('collision/geometry/cylinder/length').text = '{:.4f}'.format(aff.getDescription()['Length']) link.append(etree.Element('visual', {'name': 'visual'})) link.find('visual').append(etree.Element('geometry')) link.find('visual/geometry').append(etree.Element('cylinder')) link.find('visual/geometry/cylinder').append(etree.Element('radius')) link.find('visual/geometry/cylinder/radius').text = '{:.4f}'.format(aff.getDescription()['Radius']) link.find('visual/geometry/cylinder').append(etree.Element('length')) link.find('visual/geometry/cylinder/length').text = '{:.4f}'.format(aff.getDescription()['Length']) link.find('visual').append(etree.Element('material')) link.find('visual/material').append(etree.Element('diffuse')) link.find('visual/material/diffuse').text = '{:s} {:.1f}'.format(' '.join(map(str, aff.getDescription()['Color'])), aff.getDescription()['Alpha']) link.find('visual/material').append(etree.Element('ambient')) link.find('visual/material/ambient').text = '0 0 0 1' return link
def generateBoxLinkNode(self, aff): link = etree.Element('link', {'name':aff.getDescription()['Name']}) link.append(etree.Element('pose')) pose = np.append(aff.getDescription()['pose'][0], transformUtils.quaternionToRollPitchYaw(aff.getDescription()['pose'][1])) link.find('pose').text = ' '.join(map(str, pose)) dimensions = aff.getDescription()['Dimensions'] if aff.getDescription()['Collision Enabled']: link.append(etree.Element('collision', {'name': 'collision'})) link.find('collision').append(etree.Element('geometry')) link.find('collision/geometry').append(etree.Element('box')) link.find('collision/geometry/box').append(etree.Element('size')) link.find('collision/geometry/box/size').text = ' '.join(map(str, dimensions)) link.append(etree.Element('visual', {'name': 'visual'})) link.find('visual').append(etree.Element('geometry')) link.find('visual/geometry').append(etree.Element('box')) link.find('visual/geometry/box').append(etree.Element('size')) link.find('visual/geometry/box/size').text = ' '.join(map(str, dimensions)) link.find('visual').append(etree.Element('material')) link.find('visual/material').append(etree.Element('diffuse')) link.find('visual/material/diffuse').text = '{:s} {:.1f}'.format(' '.join(map(str, aff.getDescription()['Color'])), aff.getDescription()['Alpha']) link.find('visual/material').append(etree.Element('ambient')) link.find('visual/material/ambient').text = '0 0 0 1' return link
def generateBoxLinkNode(self, aff): link = etree.Element("link", {"name": aff.getDescription()["Name"]}) link.append(etree.Element("pose")) pose = np.append( aff.getDescription()["pose"][0], transformUtils.quaternionToRollPitchYaw(aff.getDescription()["pose"][1]) ) link.find("pose").text = " ".join(map(str, pose)) dimensions = aff.getDescription()["Dimensions"] if aff.getDescription()["Collision Enabled"]: link.append(etree.Element("collision", {"name": "collision"})) link.find("collision").append(etree.Element("geometry")) link.find("collision/geometry").append(etree.Element("box")) link.find("collision/geometry/box").append(etree.Element("size")) link.find("collision/geometry/box/size").text = " ".join(map(str, dimensions)) link.append(etree.Element("visual", {"name": "visual"})) link.find("visual").append(etree.Element("geometry")) link.find("visual/geometry").append(etree.Element("box")) link.find("visual/geometry/box").append(etree.Element("size")) link.find("visual/geometry/box/size").text = " ".join(map(str, dimensions)) link.find("visual").append(etree.Element("material")) link.find("visual/material").append(etree.Element("diffuse")) link.find("visual/material/diffuse").text = "{:s} {:.1f}".format( " ".join(map(str, aff.getDescription()["Color"])), aff.getDescription()["Alpha"] ) link.find("visual/material").append(etree.Element("ambient")) link.find("visual/material/ambient").text = "0 0 0 1" return link
def generateCylinderLinkNode(self, aff): link = etree.Element("link", {"name": aff.getDescription()["Name"]}) link.append(etree.Element("pose")) pose = np.append( aff.getDescription()["pose"][0], transformUtils.quaternionToRollPitchYaw(aff.getDescription()["pose"][1]) ) link.find("pose").text = " ".join(map(str, pose)) if aff.getDescription()["Collision Enabled"]: link.append(etree.Element("collision", {"name": "collision"})) link.find("collision").append(etree.Element("geometry")) link.find("collision/geometry").append(etree.Element("cylinder")) link.find("collision/geometry/cylinder").append(etree.Element("radius")) link.find("collision/geometry/cylinder/radius").text = "{:.4f}".format(aff.getDescription()["Radius"]) link.find("collision/geometry/cylinder").append(etree.Element("length")) link.find("collision/geometry/cylinder/length").text = "{:.4f}".format(aff.getDescription()["Length"]) link.append(etree.Element("visual", {"name": "visual"})) link.find("visual").append(etree.Element("geometry")) link.find("visual/geometry").append(etree.Element("cylinder")) link.find("visual/geometry/cylinder").append(etree.Element("radius")) link.find("visual/geometry/cylinder/radius").text = "{:.4f}".format(aff.getDescription()["Radius"]) link.find("visual/geometry/cylinder").append(etree.Element("length")) link.find("visual/geometry/cylinder/length").text = "{:.4f}".format(aff.getDescription()["Length"]) link.find("visual").append(etree.Element("material")) link.find("visual/material").append(etree.Element("diffuse")) link.find("visual/material/diffuse").text = "{:s} {:.1f}".format( " ".join(map(str, aff.getDescription()["Color"])), aff.getDescription()["Alpha"] ) link.find("visual/material").append(etree.Element("ambient")) link.find("visual/material/ambient").text = "0 0 0 1" return link
def convertStateMessageToDrakePose(msg, strict=True): """ If strict is true, then the state message must contain a joint_position for each named joint in the drake pose joint names. If strict is false, then a default value of 0.0 is used to fill joint positions that are not specified in the robot state msg argument. """ jointMap = {} for name, position in zip(msg.joint_name, msg.joint_position): jointMap[name] = position jointPositions = [] for name in getDrakePoseJointNames()[6:]: if strict: jointPositions.append(jointMap[name]) else: jointPositions.append(jointMap.get(name, 0.0)) 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 setRobotPoseFromOptitrack(robotToBodyOffset=[0.0, -0.035, 0.0, -90, -90.5, 0], optitrackBodyName='Body 1'): bodyToWorld = om.findObjectByName(optitrackBodyName + ' frame').transform robotToBody = transformUtils.frameFromPositionAndRPY(robotToBodyOffset[:3], robotToBodyOffset[3:]) robotToWorld = transformUtils.concatenateTransforms([robotToBody, bodyToWorld]) pos, quat = transformUtils.poseFromTransform(robotToWorld) rpy = transformUtils.quaternionToRollPitchYaw(quat) robotSystem.robotStateJointController.q[:6] = np.hstack((pos,rpy)) robotSystem.robotStateJointController.push()
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 generateCylinderLinkNode(self, aff): link = etree.Element('link', {'name': aff.getDescription()['Name']}) link.append(etree.Element('pose')) pose = np.append( aff.getDescription()['pose'][0], transformUtils.quaternionToRollPitchYaw( aff.getDescription()['pose'][1])) link.find('pose').text = ' '.join(map(str, pose)) if aff.getDescription()['Collision Enabled']: link.append(etree.Element('collision', {'name': 'collision'})) link.find('collision').append(etree.Element('geometry')) link.find('collision/geometry').append(etree.Element('cylinder')) link.find('collision/geometry/cylinder').append( etree.Element('radius')) link.find( 'collision/geometry/cylinder/radius').text = '{:.4f}'.format( aff.getDescription()['Radius']) link.find('collision/geometry/cylinder').append( etree.Element('length')) link.find( 'collision/geometry/cylinder/length').text = '{:.4f}'.format( aff.getDescription()['Length']) link.append(etree.Element('visual', {'name': 'visual'})) link.find('visual').append(etree.Element('geometry')) link.find('visual/geometry').append(etree.Element('cylinder')) link.find('visual/geometry/cylinder').append(etree.Element('radius')) link.find('visual/geometry/cylinder/radius').text = '{:.4f}'.format( aff.getDescription()['Radius']) link.find('visual/geometry/cylinder').append(etree.Element('length')) link.find('visual/geometry/cylinder/length').text = '{:.4f}'.format( aff.getDescription()['Length']) link.find('visual').append(etree.Element('material')) link.find('visual/material').append(etree.Element('diffuse')) link.find('visual/material/diffuse').text = '{:s} {:.1f}'.format( ' '.join(map(str, aff.getDescription()['Color'])), aff.getDescription()['Alpha']) link.find('visual/material').append(etree.Element('ambient')) link.find('visual/material/ambient').text = '0 0 0 1' return link
def generateCylinderLinkNode(self, aff): link = etree.Element("link", {"name": aff.getDescription()["Name"]}) link.append(etree.Element("pose")) pose = np.append( aff.getDescription()["pose"][0], transformUtils.quaternionToRollPitchYaw( aff.getDescription()["pose"][1]), ) link.find("pose").text = " ".join(map(str, pose)) if aff.getDescription()["Collision Enabled"]: link.append(etree.Element("collision", {"name": "collision"})) link.find("collision").append(etree.Element("geometry")) link.find("collision/geometry").append(etree.Element("cylinder")) link.find("collision/geometry/cylinder").append( etree.Element("radius")) link.find( "collision/geometry/cylinder/radius").text = "{:.4f}".format( aff.getDescription()["Radius"]) link.find("collision/geometry/cylinder").append( etree.Element("length")) link.find( "collision/geometry/cylinder/length").text = "{:.4f}".format( aff.getDescription()["Length"]) link.append(etree.Element("visual", {"name": "visual"})) link.find("visual").append(etree.Element("geometry")) link.find("visual/geometry").append(etree.Element("cylinder")) link.find("visual/geometry/cylinder").append(etree.Element("radius")) link.find("visual/geometry/cylinder/radius").text = "{:.4f}".format( aff.getDescription()["Radius"]) link.find("visual/geometry/cylinder").append(etree.Element("length")) link.find("visual/geometry/cylinder/length").text = "{:.4f}".format( aff.getDescription()["Length"]) link.find("visual").append(etree.Element("material")) return link
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 to_xyzrpy(pose): pos = pose[:3] quat = pose[6], pose[3], pose[4], pose[5] rpy = transformUtils.quaternionToRollPitchYaw(quat) return np.hstack([pos, rpy])
def getRollPitchYawFromRobotState(robotState): return transformUtils.quaternionToRollPitchYaw(robotState[3:7])