Source code for klampt.model.types

"""Utilities for inspecting Klamp't objects to retrieve a type string / create
objects from type strings.

from import ContactPoint,Hold
from ..model.trajectory import Trajectory,RobotTrajectory,SO3Trajectory,SE3Trajectory
from ..model.multipath import MultiPath
from ..math import vectorops,so3,se3
from ..robotsim import WorldModel,RobotModel,RobotModelLink,RigidObjectModel,TerrainModel,IKObjective,Geometry3D,TriangleMesh,PointCloud,GeometricPrimitive,ConvexHull,VolumeGrid
import warnings

_knownTypes = set(['Value','Vector2','Vector3','Matrix3','Point','Rotation','RigidTransform','Vector','Config',

_vectorLikeTypes = set(['Vector2','Vector3','Matrix3','Point','Rotation','Vector','Config'])
_arrayLikeTypes = set(['Vector2','Vector3','Matrix3','Point','Rotation','RigidTransform','Vector','Config','IntArray','StringArray','Configs'])
_pathLikeTypes = set(['Configs','Trajectory','LinearPath','MultiPath','SE3Trajectory','SO3Trajectory'])
_geometryTypes = set(['TriangleMesh','PointCloud','VolumeGrid','GeometricPrimitive','ConvexHull','Geometry3D'])

[docs]def known_types(): """Returns a set of all known Klampt types""" global _knownTypes return _knownTypes
[docs]def object_to_types(object,world=None): """Returns a string defining the type of the given Python Klamp't object. If multiple types could be associated with it, then it returns a list of all possible valid types.""" if isinstance(object,ContactPoint): return 'ContactPoint' elif isinstance(object,Hold): return 'Hold' elif isinstance(object,IKObjective): return 'IKGoal' elif isinstance(object,SE3Trajectory): return ['SE3Trajectory','Trajectory'] elif isinstance(object,SO3Trajectory): return ['SO3Trajectory','Trajectory'] elif isinstance(object,Trajectory): return 'Trajectory' elif isinstance(object,MultiPath): return 'MultiPath' elif isinstance(object,GeometricPrimitive): return 'GeometricPrimitive' elif isinstance(object,TriangleMesh): return 'TriangleMesh' elif isinstance(object,PointCloud): return 'PointCloud' elif isinstance(object,VolumeGrid): return 'VolumeGrid' elif isinstance(object,ConvexHull): return 'ConvexHull' elif isinstance(object,Geometry3D): return ['Geometry3D',object.type()] elif isinstance(object,WorldModel): return 'WorldModel' elif isinstance(object,RobotModel): return 'RobotModel' elif isinstance(object,RigidObjectModel): return 'RigidObjectModel' elif isinstance(object,TerrainModel): return 'TerrainModel' #this was here for Geometry3D, but might be mistaken with a SimRobotSensor. #elif hasattr(object,'type'): # if callable(object.type): # return object.type() # return object.type elif hasattr(object,'__iter__'): if len(object)>0 and hasattr(object[0],'__iter__'): if isinstance(object[0],str): if not all(isinstance(v,str) for v in object): raise ValueError("Mixing string and other items in sequence?") return 'StringArray' #list of lists or tuples if len(object)==2: if len(object[0])==9 and len(object[1])==3: #se3 return 'RigidTransform' allequal = True for entry in object: if len(entry) != len(object[0]): allequal = False break if allequal: return 'Configs' raise ValueError("Sequence of unequal-length types passed to object_to_types") else: dtypes = [] if any(isinstance(v,int) for v in object): dtypes.append('int') if any(isinstance(v,float) for v in object): dtypes.append('float') if any(isinstance(v,str) for v in object): dtypes.append('str') vtypes = [] if not str in dtypes: vtypes.append('Config') if not 'float' in dtypes: vtypes.append('IntArray') if len(object)==2: #2d point vtypes.append('Vector2') elif len(object)==3: #3d point vtypes.append('Vector3') elif len(object)==9: #so3 or 9d point? vtypes.append('Matrix3') if vectorops.distance(so3.mul(so3.inv(object),object),so3.identity())<1e-5: vtypes.append('Rotation') else: vtypes.append("StringArray") if len(vtypes)==1: return vtypes[0] return vtypes elif isinstance(object,(int,float)): return 'Value' else: raise ValueError("Unknown object of type %s passed to object_to_types"%(object.__class__.__name__,))
[docs]def object_to_type(obj,validTypes=None,world=None): """Returns a type string for the Klamp't object obj, restricted to the set of validTypes. If there are multiple interpretations, the first type in object_to_types that matches a valid type is returned. Args: obj: A Klamp't-compatible object validTypes (set, dict, or None): a set or dict of possible valid types. If None, any type is accepted Returns: str or None: The type of the object, or None if no valid type was found """ otypes = object_to_types(obj,world) if isinstance(otypes,list): if validTypes is None: return otypes[0] for otype in otypes: if otype in validTypes: return otype return None else: #only one type return otypes
[docs]def make(type,object=None): """Makes a default instance of the given type. Args: type (str): the name of the desired type object (optional): If ``type`` is 'Config', 'Configs', 'Vector', or 'Trajectory', can provide the object for which the new instance will be compatible. """ if type == 'Config' or type == 'Vector': if isinstance(object,RobotModel): return object.getConfig() else: from . import config return config.getConfig(object) elif type == 'Configs': return [make('Config',object)] elif type == 'Trajectory': if object is None: return Trajectory() elif isinstance(object,RobotModel): return RobotTrajectory(object,[0.0],make('Configs',object)) else: types = object_to_types(object) if types == 'RigidTransform': return SE3Trajectory([0.0],make('Configs',object)) elif 'Matrix3' in types: return SO3Trajectory([0.0],make('Configs',object)) else: return Trajectory([0.0],make('Configs',object)) elif type == 'IKGoal': return IKObjective() elif type == 'Vector2': return [0.,0.] elif type == 'Vector3' or type == 'Point': return [0.,0.,0.] elif type == 'Rotation' or type == 'Matrix3': return so3.identity() elif type == 'RigidTransform': return se3.identity() elif type == 'ContactPoint': return ContactPoint() elif type == 'Hold': return Hold() elif type == 'LinearPath': return Trajectory() elif type == 'MultiPath': return MultiPath() elif type == 'SO3Trajectory': return SO3Trajectory() elif type == 'SE3Trajectory': return SE3Trajectory() elif type == 'Value': return 0 elif type == 'Geometry3D': return Geometry3D() elif type == 'TriangleMesh': return TriangleMesh() elif type == 'PointCloud': return PointCloud() elif type == 'GeometricPrimitive': p = GeometricPrimitive() p.setPoint((0,0,0)) return p elif type == 'VolumeGrid': return VolumeGrid() elif type == 'ConvexHull': return ConvexHull() elif type == 'IntArray': return [0] elif type == 'StringArray': return [''] elif type == 'WorldModel': return WorldModel() elif type in ['RobotModel','RigidObjectModel','TerrainModel']: if isinstance(object,WorldModel): if type == 'RobotModel': return object.makeRobot('Untitled') elif type == 'RigidObjectModel': return object.makeRigidObject('Untitled') else: return object.makeTerrain('Untitled') raise ValueError("Can't make an independent robot, rigid object, or terrain") else: raise ValueError("Can't make a Klamp't object of type %s"%(type,)) return None
[docs]def info(object,world=None) -> dict: """Returns a dictionary containing metadata for the object. At a minimum, the "type" key will be filled in with the object's type, or None if the Klampt type is unknown. """ res = {} try: otypes = object_to_types(object,world) except Exception: res["type"] = None return res otype = otypes if isinstance(otypes,list): otype = otypes[0] res["possible types"] = otypes res["type"] = otype if otype == "Configs": res["items"] = len(object) if len(object) > 0: res["dofs"] = len(object[0]) elif otype in _arrayLikeTypes: res["items"] = len(object) elif otype in ["LinearPath","Trajectory","SO3Trajectory","SE3Trajectory"]: res["items"] = len(object.times) res["duration"] = object.duration() res["start time"] = object.startTime() res["end time"] = object.endTime() res["length"] = object.length() if len(object.milestones) > 0: res["dofs"] = len(object.milestones[0]) elif otype == "IKGoal": res["link"] if object.destLink() >= 0: res["destination link"] = object.destLink() else: res["destination link"] = None res["position dimensions constrained"] = object.numPosDims() res["rotation dimensions constrained"] = object.numRotDims() elif otype == "TriangleMesh": res["vertices"] = len(object.vertices)//3 res["triangles"] = len(object.indices)//3 bmin,bmax = Geometry3D(object).getBBTight() res["lower bound"] = bmin res["upper bound"] = bmax elif otype == "PointCloud": res["points"] = object.numPoints() res["properties"] = object.numProperties() bmin,bmax = Geometry3D(object).getBBTight() res["lower bound"] = bmin res["upper bound"] = bmax elif otype == "VolumeGrid": res["dims"] = [object.dims[0],object.dims[1],object.dims[2]] if len(object.values) > 0: res["minimum value"] = min(object.values) res["maximum value"] = max(object.values) res["lower bound"] = [object.bbox[0],object.bbox[1],object.bbox[2]] res["upper bound"] = [object.bbox[3],object.bbox[4],object.bbox[5]] elif otype == "Geometry3D": res["geometry type"] = object.type() res["#elements"] = object.numElements() bmin,bmax = object.getBBTight() res["lower bound"] = bmin res["upper bound"] = bmax if object.type() == "Group": res["elements"] = [info(object.getElement(i)) for i in range(object.numElements())] elif otype == 'WorldModel': res["#robots"] = object.numRobots() res["#rigid objects"] = object.numRigidObjects() res["#terrains"] = object.numTerrains() if object.numRobots(): res["robots"] = [info(object.robot(i)) for i in range(object.numRobots())] if object.numRigidObjects(): res["rigid objects"] = [info(object.rigidObject(i)) for i in range(object.numRigidObjects())] if object.numTerrains(): res["terrains"] = [info(object.terrain(i)) for i in range(object.numTerrains())] elif otype == 'RobotModel': res["name"] = object.getName() res["#links"] = object.numLinks() res["#drivers"] = object.numDrivers() links = [{} for i in range(object.numLinks())] for i in range(object.numLinks()): links[i]["joint type"] = ("P" if else "R") links[i]["name"] = if not links[i]["geometry"] = info( res["links"] = links elif otype in ['RigidObjectModel','TerrainModel']: res["name"] = object.getName() if not object.geometry().empty(): res["geometry"] = info(object.geometry()) return res
[docs]def convert(object,dest_type): """Converts an object to a semi-equivalent destination type. Only works for path-like objects (Configs, Trajectory, MultiPath, [X]Trajectory) and geometry objects. Object data may be referenced in the result. """ global _pathLikeTypes,_geometryTypes, _vectorLikeTypes otype = object_to_type(object) if otype == dest_type: return object source_path_like = (otype in _pathLikeTypes) dest_path_like = (dest_type in _pathLikeTypes) source_geometry = (otype in _geometryTypes) dest_geometry = (dest_type in _geometryTypes) if otype in ["Config","Vector2","Vector3","Vector"] and dest_path_like: #make singleton dest = make(dest_type,object) if isinstance(dest,list): dest.append(object) elif dest_type=="MultiPath": dest.setTrajectory([object]) else: dest.times.append(0.0) dest.milestones.append(object) return dest if source_path_like and dest_type in _vectorLikeTypes: #see if singleton can be extracted if otype=='Configs' and len(object)==1: return object[1] elif otype=='MultiPath': if len(object.sections)==1 and len(object.sections[0].configs)==1: return object.sections[0].configs[0] elif len(object.milestones)==1: return object.milestones[1] raise ValueError("Can't convert a path-like object with more than one milestone to a milestone type") if source_path_like and dest_path_like: #path to path if otype == 'MultiPath': object = object.getTrajectory() otype = 'Trajectory' if otype == "Configs": dest = make(dest_type) if dest_type == 'MultiPath': dest.setTrajectory(object) else: dest.times = list(range(len(object))) dest.milestones = object[:] return dest if dest_type == "Configs": return object.milestones[:] else: dest = make(dest_type) if dest_type == 'MultiPath': dest.setTrajectory(object) else: dest.times = object.times[:] dest.milestones = object.miltesones[:] return dest if source_geometry and dest_geometry: #geometry to geometry if not isinstance(object,Geometry3D): object = Geometry3D(object) if dest_type == "Geometry3D": return object return object.convert(dest_type) raise ValueError("Invalid conversion {} -> {}".format(otype,dest_type))
[docs]def transfer(object,source_robot,target_robot,link_map=None): """Converts a Klampt object that refers to a source robot to an object that refers to a target robot, assuming matched link names. Args: object: any Klampt object. This is only meaningful for int, list of int (IntArray), vector (Config), list of vector (Configs), Trajectory, IKGoal, RobotModelLink, or SubRobotModel types. source_robot (str or RobotModel): the robot for which ``object`` is meaningful. If str, this is the filename of the source robot. target_robot (str or RobotModel): the robot that will use the return result. If str, this is the filename of the target robot. link_map (dict, optional): if given, maps source link names to target link names, or source link indices to target link indices. This can also be a 1-element dict {'*':'prefix:*'} indicating that the links of source_robot map to 'prefix:'+[LINK_NAME], or a dict {'prefix:*':'*'} indicating the reverse. Returns: same type as object: the object, with links mapped to the target robot. """ from ..robotsim import WorldModel,RobotModelLink from .subrobot import SubRobotModel temp_world = None if isinstance(source_robot,str): temp_world = WorldModel() if not temp_world.readFile(source_robot): raise ValueError("Couldn't load source robot model "+source_robot) source_robot = temp_world.robot(0) if isinstance(target_robot,str): if temp_world is None: temp_world = WorldModel() if not temp_world.readFile(target_robot): raise ValueError("Couldn't load target robot model "+target_robot) target_robot = temp_world.robot(temp_world.numRobots()-1) if link_map is None: link_map = dict() for i in range(source_robot.numLinks()): sname = tlink = if tlink.index >= 0: link_map[i] = tlink.index warn_threshold = min(source_robot.numLinks()//2,target_robot.numLinks()//2) if len(link_map) < warn_threshold: print("klampt.model.types.transfer: warning, auto-detected link map:") print(link_map) print("Source links:") for i in range(source_robot.numLinks()): print(" ", print("Target links:") for i in range(target_robot.numLinks()): print(" ", if len(link_map)==1: items = list(link_map.items()) if items[0][0] =='*': if not items[0][1].endswith('*'): if '*' not in items[0][1]: raise NotImplementedError("TODO: match * inside target string") raise ValueError("Invalid wildcard match string") prefix = items[0][1][:-1] link_map = dict() for i in range(source_robot.numLinks()): sname = tlink = if tlink.index >= 0: link_map[i] = tlink.index elif items[0][1] == '*': if not items[0][0].endswith('*'): if '*' not in items[0][0]: raise NotImplementedError("TODO: match * inside source string") raise ValueError("Invalid wildcard match string") prefix = items[0][0][:-1] link_map = dict() for i in range(target_robot.numLinks()): tname = slink = if slink.index >= 0: link_map[slink.index] = i if len(link_map)==0: raise ValueError("The source and target robot have no links in common") #convert to indices link_map_indices = dict() for (k,v) in link_map.items(): if isinstance(k,str): kindex = if kindex < 0: raise ValueError("Invalid source link "+k) k = kindex elif k < 0 or k >= source_robot.numLinks(): raise ValueError("Invalid source link {}".format(k)) if isinstance(v,str): vindex = if vindex < 0: raise ValueError("Invalid target link "+v) v = vindex elif k < 0 or k >= target_robot.numLinks(): raise ValueError("Invalid target link {}".format(k)) link_map_indices[k]=v if isinstance(object,int): return link_map_indices.get(object,-1) elif isinstance(object,RobotModelLink): return,-1)) elif isinstance(object,SubRobotModel): assert object._robot is source_robot,"Incorrect source robot for transferring SubRobotModel" tlinks = transfer(object._links,source_robot,target_robot,link_map_indices) if any(v < 0 for v in tlinks): raise ValueError("Can't transfer SubRobotModel; one or more links of the source subrobot does not appear in the target robot") return SubRobotModel(target_robot,tlinks) try: otypes = object_to_types(object,temp_world) if isinstance(otypes,str): otypes = [otypes] except ValueError: return object if 'IntArray' in otypes: return [link_map_indices.get(ind,-1) for ind in object] elif 'Config' in otypes: qres = target_robot.getConfig() for i,j in link_map_indices.items(): qres[j] = object[i] return qres elif 'Configs' in otypes: res = [] qres = target_robot.getConfig() for q in object: for i,j in link_map_indices.items(): qres[j] = q[i] res.append([v for v in qres]) return res elif 'IKGoal' in otypes: source_link,dest_link =,object.destLink() res = object.copy() res.setLinks(source_link,dest_link) return res elif 'Trajectory' in otypes: if isinstance(object,RobotTrajectory): return RobotTrajectory(target_robot,object.times,transfer(object.milestones,source_robot,target_robot,link_map_indices)) return object.constructor(object.times,transfer(object.milestones,source_robot,target_robot,link_map_indices)) else: return object
def _deprecated_func(oldName,newName): import sys mod = sys.modules[__name__] f = getattr(mod,newName) def depf(*args,**kwargs): warnings.warn("{} will be deprecated in favor of {} in a future version of Klampt".format(oldName,newName),DeprecationWarning) return f(*args,**kwargs) depf.__doc__ = 'Deprecated in a future version of Klampt. Use {} instead'.format(newName) setattr(mod,oldName,depf) _deprecated_func("knownTypes","known_types") _deprecated_func("objectToTypes","object_to_types")