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
