def writeHold(h):
text = "begin hold\n"
text += "link = "+str(h.link)+"\n"
text += "contacts = ";
text += " \\\n ".join([writeContactPoint(c) for c in h.contacts])
text += "\n"
localPos, worldPos = h.ikConstraint.getPosition()
text += "position = "+" ".join(str(v) for v in localPos)+" \\\n"
text += " "+" ".join(str(v) for v in worldPos)+"\n"
#TODO: write ik constraint rotation
if h.ikConstraint.numRotDims()==3:
#fixed rotation
m = so3.moment(h.ikConstraint.getRotation())
text += "rotation = "+" ".join(str(v) for v in m)+"\n"
elif h.ikConstraint.numRotDims()==1:
locAxis,worldAxis = h.ikConstraint.getRotationAxis()
text += "axis = "+" ".join(str(v) for v in locAxis)+" \\\n"
text += " "+" ".join(str(v) for v in worldaxis)+"\n"
text += "end"
return text
def writeHold(h):
"""Writes a Hold to a string"""
text = "begin hold\n"
text += " link = "+str(h.link)+"\n"
text += " contacts = ";
text += " \\\n ".join([writeContactPoint(c) for c in h.contacts])
text += "\n"
localPos, worldPos = h.ikConstraint.getPosition()
text += " position = "+" ".join(str(v) for v in localPos)+" \\\n"
text += " "+" ".join(str(v) for v in worldPos)+"\n"
#TODO: write ik constraint rotation
if h.ikConstraint.numRotDims()==3:
#fixed rotation
m = so3.moment(h.ikConstraint.getRotation())
text += "rotation = "+" ".join(str(v) for v in m)+"\n"
elif h.ikConstraint.numRotDims()==1:
locAxis,worldAxis = h.ikConstraint.getRotationAxis()
text += "axis = "+" ".join(str(v) for v in locAxis)+" \\\n"
text += " "+" ".join(str(v) for v in worldaxis)+"\n"
text += "end"
return text
def toJson(obj,type='auto'):
"""Converts from a Klamp't object to a structure that can be converted
to a JSON string (e.g., from json.dumps()). If 'type' is not provided,
this attempts to infer the object type automatically.
Not all objects are supported yet.
"""
if type == 'auto':
if isinstance(obj,(list,tuple)):
if all([isinstance(v,(bool,int,float)) for v in obj]):
type = 'Vector'
else:
if len(obj)==2 and len(obj[0])==9 and len(obj[1])==3:
type = 'RigidTransform'
else:
isconfigs = True
for item in obj:
if not all([isinstance(v,(bool,int,float)) for v in item]):
isconfigs = False
if isconfigs:
type = 'Configs'
else:
raise RuntimeError("Could not parse object "+str(obj))
elif isinstance(obj,(bool,int,float,str,unicode)):
type = 'Value'
elif isinstance(obj,ContactPoint):
type = 'ContactPoint'
elif isinstance(obj,IKObjective):
type = 'IKObjective'
elif isinstance(obj,Trajectory):
type = 'Trajectory'
else:
raise RuntimeError("Unknown object of type "+obj.__class__.__name)
if type in ['Config','Configs','Vector','Matrix','Matrix3','RotationMatrix','Value','IntArray','StringArray']:
return obj
elif type == 'RigidTransform':
return obj
elif type == 'ContactPoint':
return {'x':obj.x,'n':obj.n,'kFriction':kFriction}
elif type == 'Trajectory':
return {'times':obj.times,'milestones':obj.milestones}
elif type == 'IKObjective':
res = {'type':type,'link':obj.link()}
if obj.destLink() >= 0:
res['destLink'] = obj.destLink()
if obj.numPosDims()==3:
res['posConstraint']='fixed'
res['localPosition'],res['endPosition']=obj.getPosition()
elif obj.numPosDims()==2:
res['posConstraint']='linear'
res['localPosition'],res['endPosition']=obj.getPosition()
res['direction']=obj.getPositionDirection()
elif obj.numPosDims()==1:
res['posConstraint']='planar'
res['localPosition'],res['endPosition']=obj.getPosition()
res['direction']=obj.getPositionDirection()
else:
#less verbose to just eliminate this
#res['posConstraint']='free'
pass
if obj.numRotDims()==3:
res['rotConstraint']='fixed'
res['endRotation']=so3.moment(obj.getRotation())
elif obj.numRotDims()==2:
res['rotConstraint']='axis'
res['localAxis'],res['endRotation']=obj.getRotationAxis()
elif obj.numRotDims()==1:
raise NotImplementedError("twoaxis constraints are not implemented in Klampt")
else:
#less verbose to just eliminate this
#res['rotConstraint']='free'
pass
return res
elif type in writers:
return {'type':type,'data':write(obj,type)}
else:
raise RuntimeError("Unknown or unsupported type "+type)
def expmap_from_euler(ai, aj, ak, axes="sxyz"):
matrix = transformations.euler_matrix(ai, aj, ak, axes)
R = so3.from_matrix(matrix)
moment = so3.moment(R)
axis_angle_parameters = vectorops.unit(moment) + [vectorops.norm(moment)]
return axis_angle_parameters
