def loadSDF(self, filename):
sdf = pysdf.SDF(file=filename)
for model in sdf.world.models:
for link in model.links:
if hasattr(link, "submodels"):
if len(link.submodels) > 0:
print model.name + " - This is an articulated object - SKIPPING!"
break
for col in link.collisions:
t1 = transformUtils.getTransformFromNumpy(model.pose)
t2 = transformUtils.getTransformFromNumpy(link.pose)
t3 = transformUtils.getTransformFromNumpy(col.pose)
t = t1
t.PreMultiply()
t.Concatenate(t2)
t.PreMultiply()
t.Concatenate(t3)
p = transformUtils.poseFromTransform(t)
name = "" # model.name
color = col.color[0:3] if col.color is not None else [
0.8, 0.8, 0.8
]
if len(link.name) > 0: # and link.name != model.name:
name += link.name
if len(col.name) > 0 and len(link.collisions) > 1:
name += "-" + col.name
if col.geometry_type == "mesh":
print "Mesh geometry is unsupported - SKIPPING!"
if col.geometry_type == "image":
print "image geometry is unsupported - SKIPPING!"
if col.geometry_type == "height_map":
print "Height map geometry is unsupported - SKIPPING!"
if col.geometry_type == "plane":
print "Plane geometry is unsupported - SKIPPING!"
if col.geometry_type == "sphere":
print "Sphere geometry is unsupported - SKIPPING!"
if col.geometry_type == "box":
desc = dict(
classname="BoxAffordanceItem",
Name=name,
uuid=newUUID(),
pose=p,
Color=color,
Dimensions=map(
float, col.geometry_data["size"].split(" ")),
)
self.affordanceManager.newAffordanceFromDescription(
desc)
if col.geometry_type == "cylinder":
desc = dict(
classname="CylinderAffordanceItem",
Name=name,
uuid=newUUID(),
pose=p,
Color=color,
Radius=float(col.geometry_data["radius"]),
Length=float(col.geometry_data["length"]),
)
self.affordanceManager.newAffordanceFromDescription(
desc)
def loadSDF(self, filename):
sdf = pysdf.SDF(file=filename)
for model in sdf.world.models:
for link in model.links:
if hasattr(link, 'submodels'):
if len(link.submodels) > 0:
print model.name + ' - This is an articulated object - SKIPPING!'
break
for col in link.collisions:
t1 = transformUtils.getTransformFromNumpy(model.pose)
t2 = transformUtils.getTransformFromNumpy(link.pose)
t3 = transformUtils.getTransformFromNumpy(col.pose)
t = t1
t.PreMultiply()
t.Concatenate(t2)
t.PreMultiply()
t.Concatenate(t3)
p = transformUtils.poseFromTransform(t)
name = model.name
color = col.color[0:3] if col.color is not None else [
0.8, 0.8, 0.8
]
if len(link.name) > 0 and link.name != model.name:
name += '-' + link.name
if len(col.name) > 0 and len(link.collisions) > 1:
name += '-' + col.name
if col.geometry_type == 'mesh':
print 'Mesh geometry is unsupported - SKIPPING!'
if col.geometry_type == 'image':
print 'image geometry is unsupported - SKIPPING!'
if col.geometry_type == 'height_map':
print 'Height map geometry is unsupported - SKIPPING!'
if col.geometry_type == 'plane':
print 'Plane geometry is unsupported - SKIPPING!'
if col.geometry_type == 'sphere':
print 'Sphere geometry is unsupported - SKIPPING!'
if col.geometry_type == 'box':
desc = dict(classname='BoxAffordanceItem',
Name=name,
uuid=newUUID(),
pose=p,
Color=color,
Dimensions=map(
float,
col.geometry_data['size'].split(' ')))
self.affordanceManager.newAffordanceFromDescription(
desc)
if col.geometry_type == 'cylinder':
desc = dict(classname='CylinderAffordanceItem',
Name=name,
uuid=newUUID(),
pose=p,
Color=color,
Radius=float(col.geometry_data['radius']),
Length=float(col.geometry_data['length']))
self.affordanceManager.newAffordanceFromDescription(
desc)
def loadSDF(self, filename):
sdf = pysdf.SDF(file=filename)
for model in sdf.world.models:
for link in model.links:
if hasattr(link, "submodels"):
if len(link.submodels) > 0:
print model.name + " - This is an articulated object - SKIPPING!"
break
for col in link.collisions:
t1 = transformUtils.getTransformFromNumpy(model.pose)
t2 = transformUtils.getTransformFromNumpy(link.pose)
t3 = transformUtils.getTransformFromNumpy(col.pose)
t = t1
t.PreMultiply()
t.Concatenate(t2)
t.PreMultiply()
t.Concatenate(t3)
p = transformUtils.poseFromTransform(t)
name = model.name
if len(link.name) > 0 and link.name != model.name:
name += "-" + link.name
if len(col.name) > 0 and len(link.collisions) > 1:
name += "-" + col.name
if col.geometry_type == "mesh":
print "Mesh geometry is unsupported - SKIPPING!"
if col.geometry_type == "image":
print "image geometry is unsupported - SKIPPING!"
if col.geometry_type == "height_map":
print "Height map geometry is unsupported - SKIPPING!"
if col.geometry_type == "plane":
print "Plane geometry is unsupported - SKIPPING!"
if col.geometry_type == "sphere":
print "Sphere geometry is unsupported - SKIPPING!"
if col.geometry_type == "box":
desc = dict(
classname="BoxAffordanceItem",
Name=name,
uuid=newUUID(),
pose=p,
Color=[0.8, 0.8, 0.8],
Dimensions=map(float, col.geometry_data["size"].split(" ")),
)
self.affordanceManager.newAffordanceFromDescription(desc)
if col.geometry_type == "cylinder":
desc = dict(
classname="CylinderAffordanceItem",
Name=name,
uuid=newUUID(),
pose=p,
Color=[0.8, 0.8, 0.8],
Radius=float(col.geometry_data["radius"]),
Length=float(col.geometry_data["length"]),
)
self.affordanceManager.newAffordanceFromDescription(desc)
def addToView(self, view):
rootT = transformUtils.getTransformFromNumpy(self.scene.rootnode.transformation)
for node in self.scene.rootnode.children:
t = transformUtils.getTransformFromNumpy(node.transformation)
folder = om.addContainer(node.name)
for i, mesh in enumerate(node.meshes):
pd = self.getPolyDataForMesh(mesh)
obj = vis.showPolyData(pd, "%s (mesh %d)" % (node.name, i), parent=folder, view=view)
obj.actor.SetTexture(self.getTextureForMaterial(mesh.material))
obj.actor.SetUserTransform(t)
def addToView(self, view):
rootT = transformUtils.getTransformFromNumpy(self.scene.rootnode.transformation)
for node in self.scene.rootnode.children:
t = transformUtils.getTransformFromNumpy(node.transformation)
folder = om.addContainer(node.name)
for i, mesh in enumerate(node.meshes):
pd = self.getPolyDataForMesh(mesh)
obj = vis.showPolyData(pd, '%s (mesh %d)' % (node.name, i), parent=folder, view=view)
obj.actor.SetTexture(self.getTextureForMaterial(mesh.material))
obj.actor.SetUserTransform(t)
def shadowOn(self):
if self.shadowActor:
return
mat = [[1, 0, -1, 0],
[0, 1, -1, 0],
[0, 0, 0, 0],
[0, 0, 0, 1]]
shadowT = transformUtils.getTransformFromNumpy(mat)
baseTransform = self.actor.GetUserTransform()
if baseTransform:
shadowT.PreMultiply()
shadowT.Concatenate(baseTransform)
self.shadowActor = vtk.vtkActor()
self.shadowActor.SetMapper(self.mapper)
self.shadowActor.SetUserTransform(shadowT)
self.shadowActor.GetProperty().LightingOff()
self.shadowActor.GetProperty().SetColor(0, 0, 0)
for view in self.views:
view.renderer().AddActor(self.shadowActor)
def shadowOn(self):
if self.shadowActor:
return
mat = [[1, 0, -1, 0],
[0, 1, -1, 0],
[0, 0, 0, 0],
[0, 0, 0, 1]]
shadowT = transformUtils.getTransformFromNumpy(mat)
baseTransform = self.actor.GetUserTransform()
if baseTransform:
shadowT.PreMultiply()
shadowT.Concatenate(baseTransform)
self.shadowActor = vtk.vtkActor()
self.shadowActor.SetMapper(self.mapper)
self.shadowActor.SetUserTransform(shadowT)
self.shadowActor.GetProperty().LightingOff()
self.shadowActor.GetProperty().SetColor(0, 0, 0)
for view in self.views:
view.renderer().AddActor(self.shadowActor)
def forwardKinematicsExample(self):
q = np.zeros(self.rigidBodyTree.get_num_positions())
v = np.zeros(self.rigidBodyTree.get_num_velocities())
kinsol = self.rigidBodyTree.doKinematics(q,v)
t = self.rigidBodyTree.relativeTransform(kinsol, self.bodyNameToId['world'], self.bodyNameToId['leftFoot'])
tt = transformUtils.getTransformFromNumpy(t)
pos = transformUtils.transformations.translation_from_matrix(t)
quat = transformUtils.transformations.quaternion_from_matrix(t)
def forwardKinematicsExample(self):
q = np.zeros(self.rigidBodyTree.get_num_positions())
v = np.zeros(self.rigidBodyTree.get_num_velocities())
kinsol = self.rigidBodyTree.doKinematics(q,v)
t = self.rigidBodyTree.relativeTransform(kinsol, self.bodyNameToId['world'], self.bodyNameToId['leftFoot'])
tt = transformUtils.getTransformFromNumpy(t)
pos = transformUtils.transformations.translation_from_matrix(t)
quat = transformUtils.transformations.quaternion_from_matrix(t)
def loadSDF(self, filename):
sdf = pysdf.SDF(file=filename)
for model in sdf.world.models:
for link in model.links:
if hasattr(link, 'submodels'):
if len(link.submodels)>0:
print model.name+' - This is an articulated object - SKIPPING!'
break
for col in link.collisions:
t1=transformUtils.getTransformFromNumpy(model.pose)
t2=transformUtils.getTransformFromNumpy(link.pose)
t3=transformUtils.getTransformFromNumpy(col.pose)
t=t1
t.PreMultiply()
t.Concatenate(t2)
t.PreMultiply()
t.Concatenate(t3)
p = transformUtils.poseFromTransform(t)
name = model.name
color = col.color[0:3] if col.color is not None else [0.8,0.8,0.8]
if len(link.name)>0 and link.name != model.name:
name+='-'+link.name
if len(col.name)>0 and len(link.collisions)>1:
name+='-'+col.name
if col.geometry_type=='mesh':
desc = dict(classname='MeshAffordanceItem', Name=name, uuid=newUUID(), pose=p, Color=color, Filename=col.geometry_data['uri'], Scale=map(float, col.geometry_data['scale'].split(' ')))
self.affordanceManager.newAffordanceFromDescription(desc)
if col.geometry_type=='image':
print 'image geometry is unsupported - SKIPPING!'
if col.geometry_type=='height_map':
print 'Height map geometry is unsupported - SKIPPING!'
if col.geometry_type=='plane':
print 'Plane geometry is unsupported - SKIPPING!'
if col.geometry_type=='sphere':
print 'Sphere geometry is unsupported - SKIPPING!'
if col.geometry_type=='box':
desc = dict(classname='BoxAffordanceItem', Name=name, uuid=newUUID(), pose=p, Color=color, Dimensions=map(float, col.geometry_data['size'].split(' ')))
self.affordanceManager.newAffordanceFromDescription(desc)
if col.geometry_type=='cylinder':
desc = dict(classname='CylinderAffordanceItem', Name=name, uuid=newUUID(), pose=p, Color=color, Radius=float(col.geometry_data['radius']), Length = float(col.geometry_data['length']))
self.affordanceManager.newAffordanceFromDescription(desc)
def parse_transform(transform_matrix_list):
"""
Returns a vtkTransform matrix from column major list of matrix coefficients
:param transform_matrix_list:
:type transform_matrix_list:
:return:
:rtype:
"""
matrix_coeffs = np.array(transform_matrix_list) # vector of length 16
mat = np.reshape(matrix_coeffs, [4, 4], order='F')
return transformUtils.getTransformFromNumpy(mat)
def update(self, snapshot=False):
"""
Visualizes the pointclouds set to true. This should be called from the main thread
:return:
:rtype:
"""
for topic, data in self._subscribers.iteritems():
if not data['visualize']:
continue
msg = data['subscriber'].lastMsg
if msg is None:
continue
# get frame
# TransformStamped
# this might need to be called in thread
try:
T_W_pointcloud_stamped = self._tf_buffer.lookup_transform(
self._expressed_in_frame, msg.header.frame_id,
msg.header.stamp)
except:
continue
T_W_pointcloud = ros_numpy.numpify(
T_W_pointcloud_stamped.transform)
T_W_pointcloud_vtk = transformUtils.getTransformFromNumpy(
T_W_pointcloud)
pointcloud_numpy = DirectorROSVisualizer.numpy_from_pointcloud2_msg(
msg)
pointcloud_vtk = vnp.getVtkPolyDataFromNumpyPoints(
pointcloud_numpy)
pointcloud_vtk = filterUtils.transformPolyData(
pointcloud_vtk, T_W_pointcloud_vtk)
data['pointcloud'] = pointcloud_vtk
if snapshot:
name = data["name"] + " snapshot"
vis.showPolyData(pointcloud_vtk,
name,
parent=self._vis_container)
else:
vis.updatePolyData(pointcloud_vtk,
data['name'],
parent=self._vis_container)
def testEulerBotpy():
'''
Test some quaternion and euler conversions with botpy
'''
quat = transformations.random_quaternion()
rpy = transformations.euler_from_quaternion(quat)
rpy2 = botpy.quat_to_roll_pitch_yaw(quat)
quat2 = botpy.roll_pitch_yaw_to_quat(rpy)
mat = transformations.quaternion_matrix(quat)
frame = transformUtils.getTransformFromNumpy(mat)
rpy3 = transformUtils.rollPitchYawFromTransform(frame)
print quat, quat2
print rpy, rpy2, rpy3
assert isQuatEqual(quat, quat2)
assert np.allclose(rpy, rpy2)
assert np.allclose(rpy2, rpy3)
def testEulerBotpy():
'''
Test some quaternion and euler conversions with botpy
'''
quat = transformations.random_quaternion()
rpy = transformations.euler_from_quaternion(quat)
rpy2 = botpy.quat_to_roll_pitch_yaw(quat)
quat2 = botpy.roll_pitch_yaw_to_quat(rpy)
mat = transformations.quaternion_matrix(quat)
frame = transformUtils.getTransformFromNumpy(mat)
rpy3 = transformUtils.rollPitchYawFromTransform(frame)
print quat, quat2
print rpy, rpy2, rpy3
assert isQuatEqual(quat, quat2)
assert np.allclose(rpy, rpy2)
assert np.allclose(rpy2, rpy3)
def testIkPlan():
ikPlanner = robotSystem.ikPlanner
constraints = buildConstraints()
poses = ce.getPlanPoses(constraints, ikPlanner)
global robot
robot = loadRigidBodyTree(getIkUrdfFilename())
tspan = np.array([1.0, 1.0])
handLinkNames = [ikPlanner.getHandLink('left'), ikPlanner.getHandLink('right')]
footLinkNames = [robotSystem.directorConfig['leftFootLink'], robotSystem.directorConfig['rightFootLink']]
leftHandBodyId = robot.FindBody(str(handLinkNames[0])).get_body_index()
rightHandBodyId = robot.FindBody(str(handLinkNames[1])).get_body_index()
leftFootBodyId = robot.FindBody(str(footLinkNames[0])).get_body_index()
rightFootBodyId = robot.FindBody(str(footLinkNames[1])).get_body_index()
lfootContactPts, rfootContactPts = getFootContactPoints()
#lfootContactPts, rfootContactPts = = robotSystem.footstepsDriver.getContactPts()
for i in xrange(robot.get_num_bodies()):
body = robot.get_body(i)
print i, body.get_name()
assert robot.FindBodyIndex(body.get_name()) == i
q = np.zeros(robot.get_num_positions())
v = np.zeros(robot.get_num_velocities())
kinsol = robot.doKinematics(q,v)
t = robot.relativeTransform(kinsol, robot.FindBodyIndex('world'), robot.FindBodyIndex(str(footLinkNames[0])))
tt = transformUtils.getTransformFromNumpy(t)
pos = transformUtils.transformations.translation_from_matrix(t)
quat = transformUtils.transformations.quaternion_from_matrix(t)
footConstraints = []
for linkName in footLinkNames:
t = robotSystem.robotStateModel.getLinkFrame(linkName)
pos, quat = transformUtils.poseFromTransform(t)
if False and linkName == footLinkNames[0]:
pc = pydrakeik.WorldPositionConstraint(robot, robot.findLink(str(linkName)).get_body_index(), np.zeros((3,)), pos + [-10,-10,0], pos+[10,10,0], tspan)
else:
pc = pydrakeik.WorldPositionConstraint(robot, robot.findLink(str(linkName)).get_body_index(), np.zeros((3,)), pos, pos, tspan)
qc = pydrakeik.WorldQuatConstraint(robot, robot.findLink(str(linkName)).get_body_index(), quat, 0.01, tspan)
footConstraints.append(pc)
footConstraints.append(qc)
qsc = pydrakeik.QuasiStaticConstraint(robot, tspan)
qsc.setActive(True)
qsc.setShrinkFactor(1.0)
qsc.addContact([leftFootBodyId], lfootContactPts.transpose())
qsc.addContact([rightFootBodyId], rfootContactPts.transpose())
#q_seed = robot.getZeroConfiguration()
#q_seed = robotSystem.robotStateJointController.getPose('q_nom').copy()
# todo, joint costs, and other options
global options
options = pydrakeik.IKoptions(robot)
jointIndexMap = np.zeros(robotSystem.robotStateJointController.numberOfJoints)
drakePositionNames = [robot.get_position_name(i) for i in xrange(robot.get_num_positions())]
for i, name in enumerate(robotSystem.robotStateJointController.jointNames):
jointIndexMap[i] = drakePositionNames.index(name)
jointIndexMap = list(jointIndexMap)
print jointIndexMap
q_seed = np.zeros(robot.get_num_positions())
q_seed[jointIndexMap] = robotSystem.robotStateJointController.getPose('q_nom').copy()
# setup costs
costs = np.ones(robot.get_num_positions())
groupCosts = [
('Left Leg', 1e3),
('Right Leg', 1e3),
('Left Arm', 1),
('Right Arm', 1),
('Back', 1e4),
('Neck', 1e6),
]
for groupName, costValue in groupCosts:
names = robotSystem.ikPlanner.getJointGroup(groupName)
inds = [drakePositionNames.index(name) for name in names]
print costValue, names
costs[inds] = costValue
costs[drakePositionNames.index('base_x')] = 0
costs[drakePositionNames.index('base_y')] = 0
costs[drakePositionNames.index('base_z')] = 0
costs[drakePositionNames.index('base_roll')] = 1e3
costs[drakePositionNames.index('base_pitch')] = 1e3
costs[drakePositionNames.index('base_yaw')] = 0
print costs
options.setQ(np.diag(costs))
def solveAndDraw(constraints):
#print q_seed.shape
global results
global constraintList
constraintList = constraints
results = pydrakeik.InverseKin(robot, q_seed, q_seed, constraints, options)
pose = results.q_sol[0]
#print results.info[0]
pose = pose[jointIndexMap]
robotSystem.robotStateJointController.setPose('pose_end', pose)
def onFrameModified(obj):
pos, quat = transformUtils.poseFromTransform(obj.transform)
pc = pydrakeik.WorldPositionConstraint(robot, leftHandBodyId,
np.zeros((3,)),
pos,
pos, tspan)
qc = pydrakeik.WorldQuatConstraint(robot, leftHandBodyId,
quat, 0.01, tspan)
solveAndDraw([qsc, pc, qc] + footConstraints)
#solveAndDraw([pc])
frameObj = vis.updateFrame(robotSystem.robotStateModel.getLinkFrame(ikPlanner.getHandLink('left')), 'left hand frame')
frameObj.setProperty('Edit', True)
frameObj.setProperty('Scale', 0.2)
frameObj.connectFrameModified(onFrameModified)
def run(scenePointCloudOriginal,
modelPointCloudOriginal,
numDownsampledPoints=1000):
# make deep copies of pointclouds so we don't touch original data
scenePointCloud = vtk.vtkPolyData()
scenePointCloud.DeepCopy(scenePointCloudOriginal)
modelPointCloud = vtk.vtkPolyData()
modelPointCloud.DeepCopy(modelPointCloudOriginal)
# transform the data a bit
registration = GlobalRegistrationUtils
# use this if your point cloud contains invalid points with 0.0 range
scenePointCloud = registration.removeOriginPoints(scenePointCloud)
scenePointCloud = registration.shiftPointsToOrigin(scenePointCloud)
modelPointCloud = registration.shiftPointsToOrigin(modelPointCloud)
scaleFactor = registration.rescalePolyData(
[scenePointCloud, modelPointCloud])
numDownsampledPoints = min(numDownsampledPoints,
scenePointCloud.GetNumberOfPoints)
print "scaleFactor = ", scaleFactor
print "number of scene points = ", scenePointCloud.GetNumberOfPoints()
print "downsampled number of scene points = ", numDownsampledPoints
print "number of model points = ", modelPointCloud.GetNumberOfPoints()
# files where temporary output will be stored
baseName = registration.getSandboxDir()
modelPointCloudFile = os.path.join(baseName, 'model_data.txt')
scenePointCloudFile = os.path.join(baseName, 'scene_data.txt')
outputFile = os.path.join(baseName, 'goicp_output.txt')
# write the polyData to a file so that
registration.writePointsFile(modelPointCloud, modelPointCloudFile)
registration.writePointsFile(scenePointCloud, scenePointCloudFile)
goICPBaseDir = utils.getGoICPBaseDir()
# goICPConfigFile = os.path.join(goICPBaseDir, 'demo/config.txt')
goICPConfigFile = os.path.join(utils.getLabelFusionBaseDir(),
'config/go_icp_config.txt')
goICPBin = os.path.join(goICPBaseDir, 'build/GoICP')
goICPArgs = [
goICPBin, modelPointCloudFile, scenePointCloudFile,
str(numDownsampledPoints), goICPConfigFile, outputFile
]
goICPArgs = ' '.join(goICPArgs).split()
if os.path.isfile(outputFile):
print 'removing', outputFile
os.remove(outputFile)
print 'calling goicp...'
print
print ' '.join(goICPArgs)
startTime = time.time()
subprocess.check_call(goICPArgs)
elapsedTime = time.time() - startTime
print "GoICP took " + str(elapsedTime) + " seconds"
data = open(outputFile).readlines()
print data
T = np.eye(4)
T[:3, :3] = np.array(
[np.fromstring(l, sep=' ') for l in [data[1], data[2], data[3]]])
T[:3, 3] = [float(x) for x in [data[4], data[5], data[6]]]
print T
sceneToModelTransform = transformUtils.getTransformFromNumpy(T)
if True:
parent = om.getOrCreateContainer('Go-ICP')
transformedPointCloud = filterUtils.transformPolyData(
scenePointCloud, transformUtils.getTransformFromNumpy(T))
vis.showPolyData(modelPointCloud,
'model pointcloud ',
color=[0, 1, 0],
parent=parent)
vis.showPolyData(transformedPointCloud,
'aligned model pointcloud',
color=[1, 0, 0],
parent=parent)
vis.showPolyData(scenePointCloud,
'scene pointcloud goicp',
color=[0, 1, 1],
parent=parent)
return sceneToModelTransform
def getTransformFromFile(outputFile):
assert os.path.isfile(outputFile)
data = open(outputFile).readlines()
T = np.array([np.fromstring(l, sep=' ') for l in data[2:6]])
transform = transformUtils.getTransformFromNumpy(T)
return transform
def loadTransformFromFile(filename):
lines = open(filename).readlines()
data = np.array([[float(x) for x in line.split()] for line in lines[1:]])
return transformUtils.getTransformFromNumpy(data)