def from_numpy(obj, type='auto', template=None): """Converts a numpy array or multiple numpy arrays to a Klamp't object. Supports: * lists and tuples * RigidTransform: accepts a 4x4 homogeneous coordinate transform * Matrix3, Rotation: accepts a 3x3 matrix. * Configs * Trajectory: accepts a pair (times,milestones) * TriangleMesh: accepts a pair (verts,indices) * PointCloud: accepts a n x (3+k) array, where k is the # of properties * VolumeGrid: accepts a triple (bmin,bmax,array) * Geometry3D: accepts a pair (T,geomdata) """ global supportedTypes if type == 'auto' and template is not None: otype = types.objectToTypes(template) if isinstance(otype, (list, tuple)): for t in otype: if t in supportedTypes: type = t break if type == 'auto': raise ValueError('obj is not a supported type: ' + ', '.join(otype)) else: type = otype if type == 'auto': if isinstance(obj, (tuple, list)): if all(isinstance(v, np.ndarray) for v in obj): if len(obj) == 2: if len(obj[0].shape) == 1 and len(obj[1].shape) == 2: type = 'Trajectory' elif len(obj[0].shape) == 2 and len( obj[1].shape ) == 2 and obj[0].shape[1] == 3 and obj[1].shape[1] == 3: type = 'TriangleMesh' if len(obj) == 3: if obj[0].shape == (3, ) and obj[1].shape == (3, ): type = 'VolumeGrid' if type == 'auto': raise ValueError( "Can't auto-detect type of list of shapes" + ', '.join(str(v.shape) for v in obj)) else: if isinstance(obj[0], np.ndarray) and obj[0].shape == (4, 4): type = 'Geometry3D' else: raise ValueError( "Can't auto-detect type of irregular list") else: assert isinstance( obj, np.ndarray ), "Can only convert lists, tuples, and arrays from numpy" if obj.shape == (3, 3): type = 'Matrix3' elif obj.shape == (4, 4): type = 'RigidTransform' elif len(obj.shape) == 1: type = 'Config' else: raise ValueError("Can't auto-detect type of matrix of shape " + str(obj.shape)) if type not in supportedTypes: raise ValueError(type + ' is not a supported type') if type == 'RigidTransform': return se3.from_homogeneous(obj) elif type == 'Rotation' or type == 'Matrix3': return so3.from_matrix(obj) elif type == 'Trajectory': assert len(obj) == 2, "Trajectory format is (times,milestones)" times = obj[0].tolist() milestones = obj[1].tolist() if template is not None: return template.constructor()(times, milestones) from klampt.model.trajectory import Trajectory return Trajectory(times, milestones) elif type == 'TriangleMesh': from klampt import TriangleMesh res = TriangleMesh() vflat = obj[0].flatten() res.vertices.resize(len(vflat)) for i, v in enumerate(vflat): res.vertices[i] = float(v) iflat = obj[1].flatten() res.indices.resize(len(iflat)) for i, v in enumerate(iflat): res.indices[i] = int(v) return res elif type == 'PointCloud': from klampt import PointCloud assert len(obj.shape) == 2, "PointCloud array must be a 2D array" assert obj.shape[1] >= 3, "PointCloud array must have at least 3 values" points = obj[:, :3] properties = obj[:, 3:] res = PointCloud() res.setPoints(points.shape[0], points.flatten()) if template is not None: if len(template.propertyNames) != properties.shape[1]: raise ValueError( "Template object doesn't have the same properties as the numpy object" ) for i in range(len(template.propertyNames)): res.propertyNames[i] = template.propertyNames[i] else: for i in range(properties.shape[1]): res.propertyNames.append('property %d' % (i + 1)) if len(res.propertyNames) > 0: res.properties.resize(len(res.propertyNames) * points.shape[0]) if obj.shape[1] >= 3: res.setProperties(properties.flatten()) return res elif type == 'VolumeGrid': from klampt import VolumeGrid assert len(obj) == 3, "VolumeGrid format is (bmin,bmax,values)" assert len(obj[2].shape) == 3, "VolumeGrid values must be a 3D array" bmin = obj[0] bmax = obj[1] values = obj[2] res = VolumeGrid() res.bbox.append(bmin[0]) res.bbox.append(bmin[1]) res.bbox.append(bmin[2]) res.bbox.append(bmax[0]) res.bbox.append(bmax[1]) res.bbox.append(bmax[2]) res.dims.append(values.shape[0]) res.dims.append(values.shape[1]) res.dims.append(values.shape[2]) vflat = values.flatten() res.values.resize(len(vflat)) for i, v in enumerate(vflat): res.values[i] = v return res elif type == 'Group': from klampt import Geometry3D res = Geometry3D() assert isinstance( obj, (list, tuple)), "Group format is a list or tuple of Geometry3D's" for i in range(len(obj)): res.setElement(i, from_numpy(obj[i], 'Geometry3D')) return res elif type == 'Geometry3D': from klampt import Geometry3D if not isinstance(obj, (list, tuple)) or len(obj) != 2: raise ValueError("Geometry3D must be a (transform,geometry) tuple") T = from_numpy(obj[0], 'RigidTransform') geomdata = obj[1] subtype = None if template is not None: subtype = template.type() if subtype == 'PointCloud': g = Geometry3D( from_numpy(geomdata, subtype, template.getPointCloud())) else: g = Geometry3D(from_numpy(geomdata, subtype)) g.setCurrentTransform(*T) return g subtype = 'Group' if all(isinstance(v, np.ndarray) for v in geomdata): if len(geomdata) == 2: if len(geomdata[0].shape) == 1 and len(geomdata[1].shape) == 2: subtype = 'Trajectory' elif len(geomdata[0].shape) == 2 and len( geomdata[1].shape) == 2 and geomdata[0].shape[ 1] == 3 and geomdata[1].shape[1] == 3: subtype = 'TriangleMesh' if len(geomdata) == 3: if geomdata[0].shape == (3, ) and geomdata[1].shape == (3, ): subtype = 'VolumeGrid' g = Geometry3D(from_numpy(obj, subtype)) g.setCurrentTransform(*T) return g else: return obj.flatten()
def calculation(): """ this is the main function of calibration calculation color. """ joint_positions = [] data_file = open(calidata_path + 'calibration_joint_positions.txt', 'r') for line in data_file: line = line.rstrip() l = [num for num in line.split(' ')] l2 = [float(num) for num in l] joint_positions.append(l2) data_file.close() #the marker transforms here are actually just points not transformations marker_transforms = [] data_file = open(calidata_path + 'calibration_marker_transforms.txt', 'r') for line in data_file: line = line.rstrip() l = [num for num in line.split(' ')] l2 = [float(num) for num in l] marker_transforms.append(l2) data_file.close() T_marker_assumed = loader.load( 'RigidTransform', calidata_path + 'assumed_marker_world_transform.txt') T_marker_assumed = (so3.inv(T_marker_assumed[0]), T_marker_assumed[1]) world = WorldModel() res = world.readFile("robot_model_data/ur5Blocks.xml") if not res: raise RuntimeError("unable to load model") #vis.add("world",world) robot = world.robot(0) link = robot.link(7) q0 = robot.getConfig() Tcameras = [] Tlinks = [] for i in range(len(joint_positions)): q = robot.getConfig() q[1:7] = joint_positions[i][0:6] robot.setConfig(q) Tlinks.append(link.getTransform()) #vis.add("ghost"+str(i),q) #vis.setColor("ghost"+str(i),0,1,0,0.5) robot.setConfig(q0) #vis.add("MARKER",se3.mul(link.getTransform(),T_marker_assumed)) #vis.setAttribute("MARKER","width",2) for loop in range(NLOOPS if ESTIMATE_MARKER_TRANSFORM else 1): ###Calculate Tcamera in EE (only needed for transform fit..) ###Tcameras = [se3.mul(se3.inv(Tl),se3.mul(T_marker_assumed,se3.inv(Tm_c))) for (Tl,Tm_c) in zip(Tlinks,marker_transforms)] #actually using only point fit here.... if METHOD == 'point fit': Tcamera2 = point_fit_transform( [Tm_c for Tm_c in marker_transforms], [se3.mul(se3.inv(Tl), T_marker_assumed)[1] for Tl in Tlinks]) Tcamera2 = [so3.from_matrix(Tcamera2[0]), Tcamera2[1]] Tcamera = Tcamera2 #else: # Tcamera = transform_average(Tcameras) #with the Tcamera from the current iteration, calculate marker points in world Tmarkers_world = [ se3.apply(se3.mul(Tl, Tcamera), Tm_c) for (Tl, Tm_c) in zip(Tlinks, marker_transforms) ] #Tmarkers_link = [se3.mul(se3.inv(Tl),Tm) for Tl,Tm in zip(Tlinks,Tmarkers)] if ESTIMATE_MARKER_TRANSFORM: T_marker_assumed_inv = point_fit_transform( [[0, 0, 0] for Tm_c in marker_transforms], [ se3.apply(se3.mul(Tl, Tcamera2), Tm_c) for (Tl, Tm_c) in zip(Tlinks, marker_transforms) ]) T_marker_assumed_inv = [ so3.from_matrix(T_marker_assumed_inv[0]), T_marker_assumed_inv[1] ] #print T_marker_assumed_inv T_marker_assumed = T_marker_assumed_inv #print "New assumed marker position",T_marker_assumed else: pass #print "Would want new marker transform",transform_average(Tmarkers_world) #print "New estimated camera position",Tcamera SSE_t = 0 for (Tm_c, Tl) in zip(marker_transforms, Tlinks): Tm_c_est = se3.mul(se3.mul(se3.inv(Tcamera), se3.inv(Tl)), T_marker_assumed) SSE_t += vectorops.distanceSquared(Tm_c, Tm_c_est[1]) #print "RMSE rotation (radians)",np.sqrt(SSE_R/len(Tlinks)) print "RMSE translations (meters)", np.sqrt(SSE_t / len(Tlinks)) #Tcameras = [se3.mul(se3.inv(Tl),se3.mul(T_marker_assumed,se3.inv(Tm_c))) for (Tl,Tm_c) in zip(Tlinks,marker_transforms)] #Tmarkers = [se3.mul(Tl,se3.mul(Tcamera,Tm_c)) for (Tl,Tm_c) in zip(Tlinks,marker_transforms)] print "Saving to calibrated_camera_xform.txt" cali_txt_path = calidata_path + "calibrated_camera_xform.txt" loader.save(Tcamera, "RigidTransform", cali_txt_path) if ESTIMATE_MARKER_TRANSFORM: print "Saving to calibrated_marker_link_xform.txt" loader.save(T_marker_assumed, "RigidTransform", calidata_path + "calibrated_marker_world_xform.txt") #SSE_R = 0 SSE_t = 0 for (Tm_c, Tl) in zip(marker_transforms, Tlinks): Tm_c_est = se3.mul(se3.mul(se3.inv(Tcamera), se3.inv(Tl)), T_marker_assumed) SSE_t += vectorops.distanceSquared(Tm_c, Tm_c_est[1]) print "RMSE translations (meters)", np.sqrt(SSE_t / len(Tlinks))
def broadcast_tf(broadcaster, klampt_obj, frameprefix="klampt", root="world", stamp=0.): """Broadcasts Klampt frames to the ROS tf module. Args: broadcaster (tf.TransformBroadcaster): the tf broadcaster klampt_obj: the object to publish. Can be WorldModel, Simulator, RobotModel, SimRobotController, anything with a getTransform or getCurrentTransform method, or se3 element frameprefix (str): the name of the base frame for this object root (str): the name of the TF world frame. Note: If klampt_obj is a Simulator or SimRobotController, then its corresponding model will be updated. """ from klampt import WorldModel, Simulator, RobotModel, SimRobotController, SimRobotSensor if stamp == 'now': stamp = rospy.Time.now() elif isinstance(stamp, (int, float)): stamp = rospy.Time(stamp) world = None if isinstance(klampt_obj, WorldModel): world = klampt_obj elif isinstance(klampt_obj, Simulator): world = klampt_obj.model() klampt_obj.updateModel() if world is not None: prefix = frameprefix for i in xrange(world.numRigidObjects()): T = world.rigidObject(i).getTransform() q = so3.quaternion(T[0]) broadcaster.sendTransform( T[1], (q[1], q[2], q[3], q[0]), stamp, prefix + "/" + world.rigidObject(i).getName(), root) for i in xrange(world.numRobots()): robot = world.robot(i) rprefix = prefix + "/" + robot.getName() broadcast_tf(broadcaster, robot, rprefix, root) return robot = None if isinstance(klampt_obj, RobotModel): robot = klampt_obj elif isinstance(klampt_obj, SimRobotController): robot = klampt_obj.model() robot.setConfig(klampt_obj.getSensedConfig()) if robot is not None: rprefix = frameprefix for j in xrange(robot.numLinks()): p = robot.link(j).getParent() if p < 0: T = robot.link(j).getTransform() q = so3.quaternion(T[0]) broadcaster.sendTransform( T[1], (q[1], q[2], q[3], q[0]), stamp, rprefix + "/" + robot.link(j).getName(), root) else: Tparent = se3.mul(se3.inv(robot.link(p).getTransform()), robot.link(j).getTransform()) q = so3.quaternion(Tparent[0]) broadcaster.sendTransform( Tparent[1], (q[1], q[2], q[3], q[0]), stamp, rprefix + "/" + robot.link(j).getName(), rprefix + "/" + robot.link(p).getName()) return if isinstance(klampt_obj, SimRobotSensor): from ..model import sensing Tsensor_link = sensing.get_sensor_xform(klampt_obj) link = int(klampt_obj.getSetting('link')) if klampt_obj.type( ) == 'LaserRangeSensor': #the convention between Klampt and ROS is different klampt_to_ros_lidar = so3.from_matrix([[0, 1, 0], [0, 0, 1], [1, 0, 0]]) Tsensor_link = (so3.mul(Tsensor_link[0], klampt_to_ros_lidar), Tsensor_link[1]) q = so3.quaternion(Tsensor_link[0]) robot = klampt_obj.robot() broadcaster.sendTransform( Tsensor_link[1], (q[1], q[2], q[3], q[0]), stamp, frameprefix + "/" + robot.getName() + '/' + klampt_obj.name(), frameprefix + "/" + robot.link(link).getName()) return transform = None if isinstance(klampt_obj, (list, tuple)): #assume to be an SE3 element. transform = klampt_obj elif hasattr(klampt_obj, 'getTransform'): transform = klampt_obj.getTransform() elif hasattr(klampt_obj, 'getCurrentTransform'): transform = klampt_obj.getCurrentTransform() else: raise ValueError("Invalid type given to broadcast_tf: ", klampt_obj.__class__.__name__) q = so3.quaternion(transform[0]) broadcaster.sendTransform(transform[1], (q[1], q[2], q[3], q[0]), stamp, frameprefix, root) return