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 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 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)