""" Defines many functions of Klampt as symbolic Functions.
Currently implemented:
- so3, se3
- ik
- some collide functions
- RobotModel kinematics
TODO:
- RobotModel dynamics
- Trajectories
- Geometries
- support polygons
"""
from .symbolic import *
from .symbolic_linalg import *
from .. import *
from . import so3,se3
from ..model import ik,collide
from ..io import loader
import weakref
def _so3_rotation(axis,angle):
"""Symbolic version of so3.rotation"""
cm = cos(angle)
sm = sin(angle)
#m = s[r]-c[r][r]+rrt = s[r]-c(rrt-I)+rrt = cI + rrt(1-c) + s[r]
cp = so3.cross_product(axis)
R = mul(cp,sm)
R2 = [0]*9
for i in range(3):
for j in range(3):
R2[i*3+j] = axis[i]*axis[j]*(1.-cm)
R2[0] += cm
R2[4] += cm
R2[8] += cm
return R + expr(R2)
[docs]class SO3Context(Context):
"""Defines some functions in the so3 module:
- identity, matrix, inv, mul, apply, rotation, error, distance
- from_matrix, from_rpy, rpy, from_quaternion, quaternion, from_rotation_vector, rotation_vector
- eq_constraint: equality constraint necessary for SO3 variables
- quaternion_constraint: equality constraint necessary for quaternion variables
Completeness table
================= ============= ==============
Function Derivative Simplification
================= ============= ==============
identity N/A N/A
matrix Y Y
inv Y Y
mul
apply Y,Y
rotation N,Y
from_matrix Y
from_rpy
rpy
from_quaternion Y
quaternion
from_rotation_v
rotation_vector
axis
angle Y
error
distance
eq_constraint Y
quaternion_cons Y
================= ============= ==============
"""
def __init__(self):
Context.__init__(self)
self.type = Type('V',9)
Rvar = Variable("R",self.type)
Rsymb = VariableExpression(Rvar)
R1 = Variable("R1",self.type)
R2 = Variable("R2",self.type)
V3type = Type('V',3)
q = Variable('q',Type('V',4))
pointvar = Variable("point",V3type)
pointsymb = VariableExpression(pointvar)
self.identity = self.declare(expr(so3.identity()),"identity",[])
self.identity.returnType = self.type
self.identity.description = "The identity rotation"
M = Variable("M",Type('M',(3,3)))
self.from_matrix = self.declare(flatten(transpose(M)),"from_matrix",['M'])
self.from_matrix.returnType = self.type
self.from_matrix.argTypes = [M.type]
self.from_matrix.description = "Converts from a 3x3 matrix"
self.from_matrix.setDeriv(0,lambda M,dM:self.from_matrix(dM),asExpr=True)
self.matrix = self.declare(expr(so3.matrix(Rsymb)),"matrix",["R"])
self.matrix.returnType = self.from_matrix.argTypes[0]
self.matrix.argTypes = [self.from_matrix.returnType]
self.matrix.addSimplifier(['so3.identity'],(lambda R:eye(3)),pre=True)
self.matrix.description = "Converts to a 3x3 matrix"
self.matrix.setDeriv(0,lambda R,dR:self.matrix(dR),asExpr=True)
self.from_matrix.properties['inverse'] = weakref.proxy(self.matrix)
self.matrix.properties['inverse'] = weakref.proxy(self.from_matrix)
self.inv = self.declare(expr(so3.inv(Rsymb)),"inv",["R"])
self.inv.description = "Inverts a rotation"
self.inv.setDeriv(0,lambda R,dR:self.inv(dR),asExpr=True)
self.inv.properties['inverse'] = weakref.proxy(self.inv)
self.inv.returnType = self.type
self.inv.argTypes = [self.type]
self.inv.addSimplifier(['so3.identity'],lambda R:R)
self.mul = self.declare(so3.mul,"mul")
self.mul.description = "Inverts a rotation"
self.mul.returnType = self.type
self.mul.argTypes = [self.type,self.type]
self.mul.setDeriv(0,lambda R1,R2,dR1:self.mul(dR1,R2),asExpr=True)
self.mul.setDeriv(1,lambda R1,R2,dR2:self.mul(R1,dR2),asExpr=True)
self.mul.addSimplifier(['so3.identity',None],(lambda R1,R2:R2),pre=True)
self.mul.addSimplifier([None,'so3.identity'],(lambda R1,R2:R1),pre=True)
self.mul.properties['associative'] = True
self.apply = self.declare(expr(so3.apply(Rsymb,pointsymb)),"apply",["R","point"])
self.apply.addSimplifier(['so3.identity',None],(lambda R,point:point),pre=True)
self.apply.addSimplifier([None,'zero'],(lambda R,point:point),pre=True)
self.apply.returnType = V3type
self.apply.argTypes = [self.type,V3type]
self.apply.autoSetJacobians()
self.rotation = self.declare(so3.rotation,"rotation")
self.rotation.returnType = self.type
self.rotation.argTypes = [V3type,Numeric]
self.rotation.setDeriv(1,lambda axis,angle:so3.cross_product(axis))
self.rotation.addSimplifier([None,'zero'],(lambda axis,angle:self.identity),pre=True)
self.from_rpy = self.declare(so3.from_rpy,"from_rpy")
self.from_rpy.returnType = self.type
self.from_rpy.argTypes = [V3type]
self.rpy = self.declare(so3.rpy,"rpy")
self.rpy.returnType = self.from_rpy.argTypes[0]
self.rpy.argTypes = [self.from_rpy.returnType]
self.from_rpy.properties['inverse'] = weakref.proxy(self.rpy)
self.rpy.properties['inverse'] = weakref.proxy(self.from_rpy)
self.from_quaternion = self.declare(expr(so3.from_quaternion([q[0],q[1],q[2],q[3]])),"from_quaternion",["q"])
self.from_quaternion.returnType = self.type
self.from_quaternion.argTypes = [Type('V',4)]
self.quaternion = self.declare(so3.quaternion,"quaternion")
self.quaternion.returnType = self.from_quaternion.argTypes[0]
self.quaternion.argTypes = [self.from_quaternion.returnType]
self.from_quaternion.properties['inverse'] = weakref.proxy(self.quaternion)
self.quaternion.properties['inverse'] = weakref.proxy(self.from_quaternion)
self.from_rotation_vector = self.declare(so3.from_rotation_vector,"from_rotation_vector")
self.from_rotation_vector.returnType = self.type
self.from_rotation_vector.argTypes = [V3type]
self.rotation_vector = self.declare(so3.rotation_vector,"rotation_vector")
self.rotation_vector.returnType = self.from_rotation_vector.argTypes[0]
self.rotation_vector.argTypes = [self.from_rotation_vector.returnType]
self.from_rotation_vector.properties['inverse'] = weakref.proxy(self.rotation_vector)
self.rotation_vector.properties['inverse'] = weakref.proxy(self.from_rotation_vector)
self.axis = self.declare(unit(self.rotation_vector(Rvar)),"rotation",["R"])
self.axis.returnType = V3type
self.axis.argTypes = [self.type]
self.angle = self.declare(so3.angle,"angle")
self.angle.returnType = Numeric
self.angle.argTypes = [self.type]
def angle_jacobian(R):
cosangle = (R[0]+R[4]+R[8]-1)*0.5
angle = arccos(cosangle)
#dangle / dR[0] = -1.0/sqrt(1-cosangle**2) * dcosangle/dR[0]
dacos = -1.0/sqrt(1-cosangle**2)
x = 0.5*dacos
return expr([[x,0,0,0,x,0,0,0,x]])
self.angle.setJacobian(0,angle_jacobian,asExpr=True)
self.error = self.declare(so3.error,"error")
self.error.returnType = V3type
self.error.argTypes = [self.type,self.type]
self.distance = self.declare(self.angle(self.mul(self.inv(R1),R2)),"distance",['R1','R2'])
self.distance.returnType = Numeric
self.distance.argTypes = [self.type,self.type]
self.distance.autoSetJacobians()
self.distance.properties['nonnegative'] = True
Rm = self.matrix(Rsymb)
self.eq_constraint = self.declare(dot(Rm.T,Rm),'eq_constraint',['R'])
self.quaternion_constraint = self.declare(norm2(q)-1,'quaternion_constraint',['q'])
def sym_canonical(v):
x=v[0]
y=v[1]
z=v[2]
one_plus_x = x+1
return if_(abs_(one_plus_x) <= 1e-5,
array(x,y,z,-y,x,0,-z,0,0),
array(x,y,z,-y,x + z*z/one_plus_x,-y*z/one_plus_x,-z,-y*z/one_plus_x,x + y*y/one_plus_x))
v = Variable("v",V3type)
self.canonical = self.declare(sym_canonical(v).simplify(),'canonical',['v'])
self.canonical.argTypes = [V3type]
self.canonical.returnType = self.type
self.canonical.autoSetJacobians()
[docs]class SE3Context(Context):
"""Defines some functions in the se3 module under the se3 namespace
- make(R,t): makes an SE3 element from a rotation and a translation (equivalent to list(R,t))
- identity, homogeneous, matrix (alias for homogeneous), inv, mul, apply
- from_homogeneous: converts from a 4x4 matrix
- rotation(T): retrieves the rotation corresponding to T
- translation(T): retrieves the translation corresponding to T
Completeness table
================= ============= ==============
Function Derivative Simplification
================= ============= ==============
make Y N/A
identity Y
homogeneous Y
matrix Y
from_homogeneou Y
inv Y
mul
apply N,Y
rotation Y Y
translation Y Y
================= ============= ==============
"""
def __init__(self):
Context.__init__(self)
self.Rtype = Type('V',9)
self.ttype = Type('V',3)
self.type = Type('L',2,[self.Rtype,self.ttype])
T = Variable("T",self.type)
R = Variable("R",self.Rtype)
t = Variable("t",self.ttype)
self.make = self.declare(array(R,t),"make",['R','t'])
self.identity = self.declare(se3.identity,"identity")
self.homogeneous = self.declare(se3.homogeneous,"homogeneous")
self.homogeneous.addSimplifier(['se3.identity'],lambda T:eye(4))
Rinv = so3.inv(T[0])
self.inv = self.declare(array(Rinv,neg(so3.apply(Rinv,T[1]))),"inv",['T'])
self.inv.autoSetJacobians()
self.inv.properties['inverse'] = weakref.proxy(self.inv)
self.inv.addSimplifier(['se3.identity'],lambda T:T)
self.mul = self.declare(se3.mul,"mul")
self.mul.setDeriv(0,lambda T1,T2,dT1:self.mul(dT1,T2),asExpr=True)
self.mul.setDeriv(1,lambda T1,T2,dT2:self.mul(T1,dT2),asExpr=True)
self.mul.addSimplifier(['se3.identity',None],(lambda T1,T2:T2),pre=True)
self.mul.addSimplifier([None,'se3.identity'],(lambda T1,T2:T1),pre=True)
self.mul.properties['associative'] = True
pt = Variable('pt',self.ttype)
self.apply = self.declare(so3.apply(T[0],pt)+T[1],"apply",['T','pt'])
#self.apply.setDeriv(0,lambda T,pt,dT:array(so3.apply(dT[0],pt),dT[1]))
#self.apply.setDeriv(1,lambda T,pt,dx:so3.apply(T[0],dx))
self.apply.addSimplifier([None,'zero'],lambda T,pt:T[1])
self.apply.addSimplifier(['se3.identity',None],lambda T,pt:pt)
self.apply.autoSetJacobians()
self.rotation = self.declare(T[0],"rotation",['T'])
self.translation = self.declare(T[1],"translation",['T'])
self.make.returnType = self.type
self.homogeneous.returnType = Type('M',(4,4))
self.homogeneous.argTypes = [self.type]
self.homogeneous.setDeriv(0,lambda T,dT:array([[dT[0][0],dT[0][3],dT[0][6],dT[1][0]],
[dT[0][1],dT[0][4],dT[0][7],dT[1][1]],
[dT[0][2],dT[0][5],dT[0][8],dT[1][2]],
[0.,0.,0.,0.]]),asExpr=True)
M = Variable("M",Type('M',(4,4)))
self.from_homogeneous = self.declare(array(array(M[0][0],M[1][0],M[2][0],M[0][1],M[1][1],M[2][1],M[0][2],M[1][2],M[2][2]),array(M[0][3],M[1][3],M[2][3])),'from_homogeneous',['M'])
self.from_homogeneous.autoSetJacobians()
self.matrix = self.declare(self.homogeneous(T),"matrix",['T'])
self.make.autoSetJacobians()
self.matrix.autoSetJacobians()
self.rotation.autoSetJacobians()
self.translation.autoSetJacobians()
self.identity.returnType = self.type
self.inv.returnType = self.type
self.inv.argTypes = [self.type]
self.mul.returnType = self.type
self.mul.argTypes = [self.type,self.type]
self.apply.returnType = self.ttype
self.apply.argTypes = [self.type,self.ttype]
self.rotation.returnType = self.Rtype
self.translation.returnType = self.ttype
[docs]class IKContext(Context):
"""Performs operations on IKObjective user data objects.
Defines the functions
- link(ikobj): returns the link index of an IKObjective
- robot(ikobj): returns the RobotModel of an IKObjective
- targetPos(ikobj): returns the target position of an IKObjective
- targetRot(ikobj): returns the target rotation of an IKObjective
- targetXform(ikobj): returns the target transform of an IKObjective
- localPos(ikobj): returns the local position of an IKObjective
- worldPos(ikgoal,robot): returns the world position of the IKObjective at the robot's current configuration (same as klampt.worldPos(robot,link(ikobj),localPos(ikobj)))
- worldRot(ikgoal,robot): returns the world rotation of the IKObjective at the robot's current configuration (same as klampt.worldRot(robot,link(ikobj)))
- residual(ikgoal,robot): returns the combined position and orientation residual of an IKObjective at the robot's current configuration
Completeness table
================= ============= ==============
Function Derivative Simplification
================= ============= ==============
link N/A N/A
robot N/A N/A
targetPos N/A N/A
targetRot N/A N/A
targetXform N/A N/A
localPos N/A N/A
worldPos N/A,Y(1) N/A
worldRot N/A,Y(1) N/A
residual N/A,Y(1) N/A
================= ============= ==============
Y(1): yes, for the first derivative
"""
def __init__(self):
Context.__init__(self)
def _link(ikobj):
return ikobj.link()
def _robot(ikobj):
if not hasattr(ikobj,"robot"): return None
return ikobj.robot
def _localPos(ikobj):
return ikobj.getPosition()[0]
def _targetPos(ikobj):
return ikobj.getPosition()[1]
def _targetRot(ikobj):
return ikobj.getFixedRotation()
def _targetXform(ikobj):
return (ikobj.getFixedRotation(),ikobj.getPosition()[1])
def _worldPos(ikobj,robot):
assert hasattr(ikobj,"robot"),"IKObjective must be initialized with a RobotModel instance"
assert ikobj.robot.index == robot.index
localPos = ikobj.getPosition()[0]
return ikobj.robot.link(ikobj.link()).getWorldPosition(localPos)
def _worldPosJacobian(ikobj,robot):
assert hasattr(ikobj,"robot"),"IKObjective must be initialized with a RobotModel instance"
assert ikobj.robot.index == robot.index
localPos = ikobj.getPosition()[0]
return robot.link(ikobj.link()).getPositionJacobian(localPos)
def _worldRot(ikobj,robot):
assert hasattr(ikobj,"robot"),"IKObjective must be initialized with a RobotModel instance"
assert ikobj.robot.index == robot.index
return ikobj.robot.link(ikobj.link()).getTransform()[0]
def _worldRotJacobian(ikobj,robot):
assert hasattr(ikobj,"robot"),"IKObjective must be initialized with a RobotModel instance"
assert ikobj.robot.index == robot.index
#TODO: convert this to 9 - element form
return robot.link(ikobj.link()).getOrientationJacobian()
def _residual(ikobj,robot):
assert hasattr(ikobj,"robot"),"IKObjective must be initialized with a RobotModel instance"
assert ikobj.robot.index == robot.index
s = IKSolver(robot)
s.add(ikobj)
return s.getResidual()
def _residualJacobian(ikobj,robot):
assert hasattr(ikobj,"robot"),"IKObjective must be initialized with a RobotModel instance"
assert ikobj.robot.index == robot.index
s = IKSolver(robot)
s.add(ikobj)
jac_active = s.getJacobian()
active = s.getActiveDofs()
jac = np.zeros((len(jac_active),robot.numLinks()))
jac[:,active] = np.array(jac_active)
return jac
so3type = SO3Context().type
se3type = SE3Context().type
pointType = Type('V',3)
self.type = Type('IKObjective')
self.link = Function("link",_link,returnType='I')
self.declare(self.link)
self.link.argTypes = [self.type]
self.link.deriv = 0
self.robot = Function("robot",_robot,returnType='RobotModel')
self.declare(self.robot)
self.robot.argTypes = [self.type]
self.robot.deriv = 0
self.localPos = self.declare(_localPos,"localPos")
self.localPos.returnType = pointType
self.localPos.argTypes = [self.type]
self.localPos.deriv = 0
self.targetPos = self.declare(_targetPos,"targetPos")
self.targetPos.returnType = pointType
self.targetPos.argTypes = [self.type]
self.targetPos.deriv = 0
self.targetRot = self.declare(_targetRot,"targetRot")
self.targetRot.returnType = so3type
self.targetRot.argTypes = [self.type]
self.targetRot.deriv = 0
self.targetXform = self.declare(_targetXform,"targetXform")
self.targetXform.returnType = se3type
self.targetXform.argTypes = [self.type]
self.targetXform.deriv = 0
self.worldPos = self.declare(_worldPos,"worldPos")
self.worldPos.returnType = pointType
self.worldPos.argTypes = [self.type,Type('RobotModel')]
self.worldPos.setJacobian("robot",_worldPosJacobian)
self.worldRot = self.declare(_worldRot,"worldRot")
self.worldRot.returnType = so3type
self.worldRot.argTypes = [self.type,Type('RobotModel')]
self.worldRot.setJacobian("robot",_worldRotJacobian)
self.residual = self.declare(_residual,"residual")
self.residual.returnType = Type('V')
self.residual.argTypes = [self.type,Type('RobotModel')]
self.residual.setJacobian("robot",_residualJacobian)
[docs]class GeometryContext(Context):
"""Performs operations on Geometry3D user data objects. Derivatives of geom arguments are taken with
respect to the geometry transforms.
Defines the functions:
- geometry(object): calls the function object.geometry() (e.g., object can be a RobotModelLink)
- setTransform(geom,T): sets the current transform of the geometry and returns it.
- setCollisionMargin(geom,margin): sets the current collision of the geometry and returns it.
- bbox(geom): returns the bounding box of the geometry at its current transform.
- collision(geom1,geom2): returns True if the geometries are colliding.
- distance(geom1,geom2): returns the distance between the geometries, and if penetrating and
the two geometries support signed distance, returns the negative penetation distance.
- closestPoints(geom1,geom2): returns the pair of closest points between geom1 and geom2.
- distancePoint(geom,pt): returns the closest point from geom to pt.
- closestPoint(geom,pt): returns the closest point to pt on geom.
- rayCast(geom,src,dir): returns the distance t>=0 along the ray src + t*dir that intersects geom, or
inf if there is no intersection.
NOT IMPLEMENTED YET
Completeness table
================= ============= ==============
Function Derivative Simplification
================= ============= ==============
setTransform N/A N/A
setCollisionMar N/A N/A
bbox N/A
collision Y N/A
distance N/A
closestPoints N/A
distancePoint N/A
closestPoint N/A
rayCast N/A
================= ============= ==============
"""
def __init__(self):
Context.__init__(self)
Rtype = Type('V',9)
ttype = Type('V',3)
self.Ttype = Type('L',2,[Rtype,ttype])
self.type = Type('Geometry3D')
[docs]class CollideContext(Context):
"""Defines the functions --
- robotSelfCollision(q,robot): returns True if the robot has a collision at q
- robotCollision(q,context): returns True if the robot has a collision in the world. Saves a collider into the context
- robotSelfCollisionFree(q,robot): the opposite of robotSelfCollision
- robotCollisionFree(q,context): the opposite of robotCollision
"""
def __init__(self):
Context.__init__(self)
def _robotCollision(q,context):
world = context['world']
if 'robot' in context:
robot = context['robot']
else:
robot = world.robot(0)
if 'collider' in context:
collider = context['collider']
else:
collider = collide.WorldCollider(world)
robot.setConfig(q)
for c in collider.collisions():
return True
return False
def _robotSelfCollision(q,context):
world = context['world']
if 'robot' in context:
robot = context['robot']
else:
robot = world.robot(0)
if 'collider' in context:
collider = context['collider']
else:
collider = collide.WorldCollider(world)
robot.setConfig(q)
for c in collider.robotSelfCollisions(robot):
return True
return False
_q = Variable("q",'V')
_context = UserDataExpression("context")
self.robotCollision = Function("robotCollision",_robotCollision,returnType='B')
self.robotCollision.argTypes = [Type('V'),Type('Context')]
self.robotSelfCollision = Function("robotSelfCollision",_robotSelfCollision,returnType='B')
self.robotSelfCollision.argTypes = [Type('V'),Type('Context')]
self.robotCollisionFree = Function("robotCollisionFree",not_(self.robotCollision(_q,_context)),['q','context'])
self.robotCollisionFree.argTypes = [Type('V'),Type('Context')]
self.robotSelfCollisionFree = Function("robotSelfCollisionFree",not_(self.robotSelfCollision(_q,_context)),['q','context'])
self.robotSelfCollisionFree.argTypes = [Type('V'),Type('Context')]
self.declare(self.robotCollision)
self.declare(self.robotSelfCollision)
self.declare(self.robotCollisionFree)
self.declare(self.robotSelfCollisionFree)
assert callable(self.robotCollision.func)
assert callable(self.robotSelfCollision.func)
assert isinstance(self.robotCollisionFree.func,Expression)
assert isinstance(self.robotSelfCollisionFree.func,Expression)
[docs]class KlamptContext(Context):
"""Includes all Klampt-related functions.
Namespaces:
- so3: SO3Context
- se3: SE3Context
- ik: IKContext
- collide: CollideContext
- [main]: functions to perform kinematics operations on WorldModel, RobotModel, and RobotModelLink user data objects.
The main namespace defines the functions:
- robot(world,index): returns the index'th robot in the world
- rigidObject(world,index): returns the index'th robot in the world
- terrain(world,index): returns the index'th terrain in the world
- config(robot): returns the current configuration of the robot or object
- setConfig(robot,q): sets the current configuration of the robot and returns the robot
- link(robot,index): returns the index'th link of the robot. (If you want to use a named string as the index, use const(name))
- transform(object): gets the current transform of the object
- setTransform(object,T): sets the current transform of the object and returns the object
- velocity(robot): returns the current velocity of the robot
- setVelocity(robot,dq): sets the current velocity of the robot and returns the robot
- worldPos(robot,link,localPos): returns the world position of the point on the given link in the robot's current configuration.
- localPos(robot,link,worldPos): returns the local position of the point on the given link in the robot's current configuration.
- worldRot(robot,link): returns the rotation matrix of the given link in the robot's current configuration.
- com(robot): returns the center of mass of robot at its current configuration
- gravityTorque(gravity,robot): returns the generalized gravity vector
- inJointLimits(q,robot): returns True if q is in the robot's joint limits
- getJson(object,path): returns the value of a given path under the given object's json representation. For example,
getJson(ikobjective,const("endPosition[0]")) retrieves the x coordinate of the target position of an IKObjective.
- setJson(object,path,val): returns a modified copy of the given object, where value is assigned to the given path under
the objects json representation. For example, setJson(ikobjective,const("endPosition[0]"),p) sets the x coordinate of the target
position of an IKObjective to p. Note: this operation returns a copy of the object, modified.
Also includes modules linalg, so3, se3, ik, and collide
================ ============= ==============
Function Derivative Simplification
================ ============= ==============
robot N/A N/A
rigidObject N/A N/A
terrain N/A N/A
config Y N/A
setConfig Y N/A
link N/A N/A
transform N/A
setTransform N/A
velocity N/A
setVelocity N/A
worldPos Y(1) N/A
localPos N/A
worldRot N/A
com Y(1) N/A
gravityTorque N/A
inJointLimits N/A N/A
getJson N/A N/A
setJson N/A N/A
================ ============= ==============
Y(1): yes, for the first derivative
"""
def __init__(self,world=None):
Context.__init__(self)
if world:
self.addUserData("world",world)
if world.numRobots() > 0:
self.addUserData("robot",world.robot(0))
self.include(LinAlgContext(),"linalg",modify=True)
self.include(SO3Context(),"so3",modify=True)
self.include(SE3Context(),"se3",modify=True)
self.include(IKContext(),"ik",modify=True)
self.include(CollideContext(),"collide",modify=True)
def robot(world,index):
return world.robot(index)
def rigidObject(world,index):
return world.rigidObject(index)
def terrain(world,index):
return world.terrain(index)
def config(robot):
return robot.getConfig()
def setConfig(robot,q):
robot.setConfig(q)
return robot
def link(robot,index):
return robot.link(index)
def transform(object):
return object.getConfig()
def setTransform(object,T):
object.setTransform(*T)
return object
def velocity(robot):
return robot.getVelocity()
def setVelocity(robot,dq):
robot.setVelocity(dq)
return robot
def worldPos(robot,link,localPos):
if not isinstance(link,RobotModelLink):
link = robot.link(link)
assert link.index >= 0
return link.getWorldPosition(localPos)
def worldPosJacobian_robot(robot,link,localPos,dq):
if not isinstance(link,RobotModelLink):
link = robot.link(link)
assert link.index >= 0
return link.getPositionJacobian(localPos)
def worldPosJacobian_localPos(robot,link,localPos):
if not isinstance(link,RobotModelLink):
link = robot.link(link)
assert link.index >= 0
return so3.matrix(link.getTransform()[0])
def localPos(robot,link,worldPos):
if not isinstance(link,RobotModelLink):
link = robot.link(link)
return link.getLocalPosition(worldPos)
def worldRot(robot,link):
if not isinstance(link,RobotModelLink):
link = robot.link(link)
return link.getTransform()[0]
def worldRotJacobian(robot,link):
if not isinstance(link,RobotModelLink):
link = robot.link(link)
#TODO: convert this to 9 - element form
return link.getOrientationJacobian()
def com(robot):
return robot.getCom()
def comJacobian(robot):
sumMass = 0.0
sumJacobian = np.zeros((3,robot.numLinks()))
for i in range(robot.numLinks()):
mass = robot.link(i).getMass()
sumMass += mass.getMass()
if mass.getMass() > 0:
sumJacobian += np.array(robot.link(i).getPositionJacobian(mass.getCom()))*mass.getMass()
return sumJacobian*(1.0/sumMass)
def gravityTorque(gravity,robot):
return robot.getGravityForces(gravity)
#def inJointLimits(q,robot):
# qmin,qmax = robot.getJointLimits()
# for v,a,b in zip(q,qmin,qmax):
# if v < a or v > b: return False
# return True
#def inJointLimits_simplifier(q,robot):
# if isinstance(robot,RobotModel):
# qmin,qmax = robot.getJointLimits()
# return bound_contains(qmin,qmax,q)
# return None
def str_to_path(s):
res = s.split('.[]')
for i,v in enumerate(res):
try:
res[i] = int(v)
except Exception:
pass
return res
def getJson(object,path):
assert isinstance(path,(int,str,tuple,list))
if isinstance(object,(list,dict)):
jsonobj = object
else:
jsonobj = loader.toJson(object)
if isinstance(path,str):
path = str_to_path(path)
return getJson(jsonobj,path)
elif isinstance(path,int):
return jsonobj[path]
else:
for item in path:
jsonobj = jsonobj[item]
return jsonobj
def setJson(object,path,val):
assert isinstance(path,(int,str,tuple,list))
if isinstance(object,(list,dict)):
jsonobj = object
else:
jsonobj = loader.toJson(object)
if isinstance(path,str):
path = str_to_path(path)
return setJson(jsonobj,path,val)
elif isinstance(path,int):
jsonobj[path] = val
else:
root = jsonobj
for item in path[:-1]:
root = root[item]
root[path[-1]] = val
return loader.fromJson(jsonobj)
self.worldType = Type('WorldModel')
self.robotType = Type('RobotModel')
self.rigidObjectType = Type('RigidObjectModel')
self.terrainType = Type('TerrainModel')
self.linkType = Type('RobotModelLink')
self.configType = Type('V')
self.pointType = Type('V',3)
self.robot = self.declare(robot)
self.robot.returnType = self.robotType
self.robot.argTypes = [self.worldType,Integer]
self.rigidObject = self.declare(rigidObject)
self.rigidObject.returnType = self.rigidObjectType
self.rigidObject.argTypes = [self.worldType,Integer]
self.terrain = self.declare(terrain)
self.terrain.returnType = self.terrainType
self.terrain.argTypes = [self.worldType,Integer]
self.config = self.declare(config)
self.config.returnType = self.configType
self.config.argTypes = [self.robotType]
self.config.setDeriv('robot',(lambda robot,drobot:drobot),asExpr=True,stackable=True)
self.setConfig = self.declare(setConfig)
self.setConfig.returnType = self.robotType
self.setConfig.argTypes = [self.robotType,self.configType]
self.setConfig.setDeriv('robot',0)
self.setConfig.setDeriv('q',(lambda robot,q,dq:dq),asExpr=True,stackable=True)
self.link = self.declare(link)
self.link.returnType = self.linkType
self.link.argTypes = [self.robotType,Type('I')]
self.transform = self.declare(transform)
self.transform.returnType = self.se3.type
self.transform.setDeriv('object',(lambda object,dobject:dobject),asExpr=True)
self.setTransform = self.declare(setTransform)
self.setTransform.argTypes = [Type('U'),self.se3.type]
self.setTransform.returnType = Type('U')
self.setTransform.setDeriv('object',0)
self.setTransform.setDeriv('T',(lambda object,T,dT:dT),asExpr=True)
self.velocity = self.declare(velocity)
self.velocity.returnType = self.configType
self.velocity.argTypes = [self.robotType]
self.setVelocity = self.declare(setVelocity)
self.setVelocity.returnType = self.robotType
self.setVelocity.argTypes = [self.robotType,self.configType]
self.worldPos = self.declare(worldPos)
self.worldPos.returnType = self.pointType
self.worldPos.argTypes = [self.robotType,Type('I'),self.pointType]
self.worldPos.setJacobian('robot',worldPosJacobian_robot)
self.worldPos.setJacobian('localPos',worldPosJacobian_localPos)
self.localPos = self.declare(localPos)
self.localPos.returnType = self.pointType
self.localPos.argTypes = [self.robotType,Type('I'),self.pointType]
self.worldRot = self.declare(worldRot)
self.worldRot.returnType = self.so3.type
self.worldRot.argTypes = [self.robotType,Type('I')]
self.gravityTorque = self.declare(gravityTorque)
self.com = self.declare(com)
self.com.returnType = self.pointType
self.com.argTypes = [self.robotType]
self.com.setJacobian('robot',comJacobian)
self.gravityTorque = self.declare(gravityTorque)
self.gravityTorque.argTypes = [self.pointType,self.robotType]
jl = getattr_(UserDataExpression("robot"),const("getJointLimits"))
self.inJointLimits = Function("inJointLimits",bound_contains(jl[0],jl[1],Variable("q",'V')),["q","robot"])
#self.inJointLimits = self.declare(inJointLimits)
#self.inJointLimits.returnType = Type('B')
#self.inJointLimits.argTypes = [self.configType,self.robotType]
#self.inJointLimits.simplifier = inJointLimits_simplifier
self.getJson = self.declare(getJson)
self.setJson = self.declare(setJson)
if __name__ == '__main__':
world = WorldModel()
world.readFile("../../../data/athlete_plane.xml")
ctx = Context()
ctx.include(KlamptContext(world),"klampt")
ctx.listFunctions(builtins=True)
#TODO: analytical derivatives
print("Context:",ctx.userData)
q = ctx.addVar('q','V',world.robot(0).numLinks())
jointLimitTest = ctx.klampt.inJointLimits(q,'klampt.robot')
comEval = ctx.klampt.com(q,'klampt.robot')
print(jointLimitTest)
print(comEval)
newcontext = ctx.userData.copy()
newcontext['q'] = world.robot(0).getConfig()
print(deriv(comEval,q).eval(newcontext))
qfunc,qvars = ctx.makeVectorFunction(jointLimitTest)
cmfunc,cmvars = ctx.makeVectorFunction(comEval)
print(qfunc(world.robot(0).getConfig()))
print(cmfunc(world.robot(0).getConfig()))