def integrate(self,x,d): w = [d[0],d[4],d[8]] return so3.mul(x,so3.from_moment(w))
def integrate(self,x,d): w = [d[0],d[4],d[8]] v = d[9:] return se3.mul(x,(so3.from_moment(w),v))
def fromJson(jsonobj,type='auto'): """Converts from a JSON object (e.g., from json.loads()) to a Klamp't object of the appropriate type. If 'type' is not provided, this attempts to infer the object type automatically.""" if type == 'auto': if isinstance(jsonobj,(list,tuple)): return jsonobj elif isinstance(jsonobj,(bool,int,float,str,unicode)): return jsonobj elif isinstance(jsonobj,dict): if 'type' in jsonobj: type = jsonobj["type"] elif 'times' in jsonobj and 'milestones' in jsonobj: type = 'Trajectory' elif 'x' in jsonobj and 'n' in jsonobj and 'kFriction' in jsonobj: type = 'ContactPoint' else: raise RuntimeError("Unknown JSON object of type "+jsonobj.__class__.__name) if type in ['Config','Configs','Vector','Matrix','Matrix3','RotationMatrix','Value','IntArray','StringArray']: return jsonobj elif type == 'RigidTransform': return jsonobj elif type == 'ContactPoint': return ContactPoint(jsonobj['x'],jsonobj['n'],jsonobj['kFriction']) elif type == 'Trajectory': return Trajectory(jsonobj['times'],jsonobj['milestones']) elif type == 'IKObjective': link = jsonobj['link'] destlink = jsonobj['destLink'] if 'destLink' in jsonobj else -1 posConstraint = 'free' rotConstraint = 'free' localPosition = endPosition = None direction = None endRotation = None localAxis = None if 'posConstraint' in jsonobj: posConstraint = jsonobj['posConstraint'] if 'rotConstraint' in jsonobj: rotConstraint = jsonobj['rotConstraint'] if posConstraint == 'planar' or posConstraint == 'linear': direction = jsonobj['direction'] if posConstraint != 'free': localPosition = jsonobj['localPosition'] endPosition = jsonobj['endPosition'] if rotConstraint != 'free': endRotation = jsonobj['endRotation'] if rotConstraint == 'axis' or rotConstraint == 'twoaxis': localAxis = jsonobj['localAxis'] if posConstraint == 'free' and rotConstraint == 'free': #empty return IKObjective() elif posConstraint != 'fixed': raise NotImplementedError("Can't do non-fixed position constraints yet in Python API") if rotConstraint == 'twoaxis': raise NotImplementedError("twoaxis constraints are not implemented in Klampt") if rotConstraint == 'free': obj = IKObjective() if destlink >= 0: obj.setRelativePoint(link,destlink,localPosition,endPosition) else: obj.setFixedPoint(link,localPosition,endPosition) return obj elif rotConstraint == 'axis': obj = IKObjective() h = 0.1 lpts = [localPosition,vectorops.madd(localPosition,localAxis,h)] wpts = [endPosition,vectorops.madd(endPosition,endRotation,h)] if destlink >= 0: obj.setRelativePoints(link,destlink,lpts,wpts) else: obj.setFixedPoints(link,lpts,wpts) return obj elif rotConstraint == 'fixed': obj = IKObjective() R = so3.from_moment(endRotation) t = vectorops.sub(endPosition,so3.apply(R,localPosition)) obj.setFixedTransform(link,R,t) return obj else: raise RuntimeError("Invalid IK rotation constraint "+rotConstraint) elif type in readers: return read(type,jsonobj["data"]) else: raise RuntimeError("Unknown or unsupported type "+type)
def readIKObjective(text): """Reads an IKObjective from a string in the Klamp't native format 'link destLink posConstraintType [pos constraint items] ... rotConstraintType [rot constraint items]' where link and destLink are integers, posConstraintType is one of - N: no constraint - P: position constrained to a plane - L: position constrained to a line - F: position constrained to a point and rotConstraintType is one of - N: no constraint - T: two-axis constraint (not supported) - A: rotation constrained about axis - F: fixed rotation The [pos constraint items] contain a variable number of whitespace- separated items, dependending on posConstraintType: - N: 0 items - P: the local position xl yl zl, world position x y z on the plane, and plane normal nx,ny,nz - L: the local position xl yl zl, world position x y z on the line, and line axis direction nx,ny,nz - F: the local position xl yl zl and world position x y z The [rot constraint items] contain a variable number of whitespace- separated items, dependending on rotConstraintType: - N: 0 items - T: not supported - A: the local axis xl yl zl and the world axis x y z - F: the world rotation matrix, in moment (aka exponential map) form mx my mz (see so3.from_moment() """ items = text.split() if len(items) < 4: raise ValueError("Not enough items to unpack") link = int(items[0]) destlink = int(items[1]) ind = 2 posType = None posLocal = None posWorld = None posDirection = None rotType = None rotWorld = None rotAxis = None #read pos constraint posType = items[ind] ind += 1 if posType=='N': #no constraint pass elif posType=='P' or posType=='L': posLocal = items[ind:ind+3] ind += 3 posWorld = items[ind:ind+3] ind += 3 posDirection = items[ind:ind+3] ind += 3 elif posType=='F': posLocal = items[ind:ind+3] ind += 3 posWorld = items[ind:ind+3] ind += 3 else: raise ValueError("Invalid pos type "+posType+", must be N,P,L or F") rotType = items[ind] ind += 1 if rotType=='N': #no constraint pass elif rotType=='T' or rotType=='A': rotAxis = items[ind:ind+3] ind += 3 rotWorld = items[ind:ind+3] ind += 3 elif rotType=='F': rotWorld = items[ind:ind+3] ind += 3 else: raise ValueError("Invalid rot type "+rotType+", must be N,T,A or F") if posLocal: posLocal = [float(v) for v in posLocal] if posWorld: posWorld = [float(v) for v in posWorld] if posDirection: posDirection = [float(v) for v in posDirection] if rotLocal: rotLocal = [float(v) for v in rotLocal] if rotWorld: rotWorld = [float(v) for v in rotWorld] obj = IKObjective() obj.setLinks(link,destLink); if posType=='N': obj.setFreePosConstraint() elif posType=='F': obj.setFixedPosConstraint(posLocal,posWorld) elif posType=='P': obj.setPlanePosConstraint(posLocal,posDirection,vectorops.dot(posDirection,posWorld)) else: obj.setLinearPosConstraint(posLocal,posWorld,posDirection) if rotType == 'N': obj.setFreeRotConstraint() elif rotType == 'F': #fixed constraint R = so3.from_moment(rotWorld) obj.setFixedRotConstraint(R) elif rotType == 'A': obj.setAxialRotConstraint(rotLocal,rotWorld) else: raise NotImplementedError("Two-axis rotational constraints not supported") return obj
def integrate(self, x, d): w = [d[0], d[4], d[8]] v = d[9:] return se3.mul(x, (so3.from_moment(w), v))
def integrate(self, x, d): w = [d[0], d[4], d[8]] return so3.mul(x, so3.from_moment(w))
def readIKObjective(text): """Reads an IKObjective from text""" items = text.split() if len(items) < 4: raise ValueError("Not enough items to unpack") link = int(items[0]) destlink = int(items[1]) ind = 2 posType = None posLocal = None posWorld = None posDirection = None rotType = None rotWorld = None rotAxis = None #read pos constraint posType = items[ind] ind += 1 if posType=='N': #no constraint pass elif posType=='P' or posType=='L': posLocal = items[ind:ind+3] ind += 3 posWorld = items[ind:ind+3] ind += 3 posDirection = items[ind:ind+3] ind += 3 elif posType=='F': posLocal = items[ind:ind+3] ind += 3 posWorld = items[ind:ind+3] ind += 3 else: raise ValueError("Invalid pos type "+posType+", must be N,P,L or F") rotType = items[ind] ind += 1 if rotType=='N': #no constraint pass elif rotType=='T' or rotType=='A': rotAxis = items[ind:ind+3] ind += 3 rotWorld = items[ind:ind+3] ind += 3 elif rotType=='F': rotWorld = items[ind:ind+3] ind += 3 else: raise ValueError("Invalid rot type "+rotType+", must be N,T,A or F") if posType != 'F': raise NotImplementedError("Python API can only do point and fixed IK goals") obj = IKObjective() if rotType == 'N': #point constraint obj.setRelativePoint(link,destlink,[float(v) for v in posLocal],[float(v) for v in posWorld]) elif rotType == 'F': #fixed constraint R = so3.from_moment([float(v) for v in rotWorld]) t = vectorops.sub([float(v) for v in posWorld],so3.apply(R,[float(v) for v in posLocal])) obj.setRelativeTransform(link,destlink,R,t) elif rotType == 'A': #TODO: rotational axis constraints actually can be set in the python API raise NotImplementedError("Axis rotations not yet implemented") else: raise NotImplementedError("Two-axis rotational constraints not supported") return obj