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])
def getRollPitchYawFromRobotState(robotState):
return transformUtils.quaternionToRollPitchYaw(robotState[3:7])
