====================  =============  ======================================
Function              Derivative     Notes
====================  =============  ======================================
ConfigInterpolate     N              qinterp[robot](a,b,u) -> R^n
====================  =============  ======================================

Note that each call to WorldX or LocalX will recompute the forward kinematics
of the robot, which can be rather expensive if many of these will be called.

Also, see the :class:`KinematicsBuilder` class which creates the computational
graph for a robot's forward kinematics, which allows you to reuse
subexpressions for multiple forward kinematics calls.
"""

import numpy as np
from .. import vectorops,so3,se3

[docs] def n_args(self): return 1
[docs] def n_out(self): return 3
[docs] def eval(self,q): self.robot.setConfig(q.tolist()) return np.array(self.link.getWorldPosition(self.localPos))
[docs] def derivative(self,arg,q): assert arg == 0 self.robot.setConfig(q.tolist()) return np.array(self.link.getPositionJacobian(self.localPos))
[docs] def gen_derivative(self,arg,q): if len(arg) == 1: return self.derivative(arg,q) elif len(arg) == 2: self.robot.setConfig(q.tolist()) Hx,Hy,Hz = self.link.getPositionHessian(self.localPos) return np.array([Hx,Hy,Hz]) else: raise NotImplementedError()
[docs] def n_args(self): return 1
[docs] def n_out(self): return 3
[docs] def eval(self,q): self.robot.setConfig(q.tolist()) return np.array(self.link.getWorldDirection(self.localDir))
[docs] def derivative(self,arg,q): assert arg == 0 self.robot.setConfig(q.tolist()) Jo = self.link.getOrientationJacobian() for i in range(3): Jo[i] = np.array(Jo[i]) return np.array(vectorops.cross(Jo,self.localDir))
[docs] def gen_derivative(self,arg,q): if len(arg) == 1: return self.derivative(arg[0],q) elif len(arg) == 2: self.robot.setConfig(q.tolist()) Hx,Hy,Hz = self.link.getOrientationHessian() Hx = np.array(Hx) Hy = np.array(Hy) Hz = np.array(Hz) return np.array(vectorops.cross([Hx,Hy,Hz],self.localDir)) else: raise NotImplementedError()
[docs] def n_args(self): return 1
[docs] def n_out(self): return 9
[docs] def eval(self,q): self.robot.setConfig(q.tolist()) return np.array(self.link.getTransform()[0])
[docs] def derivative(self,arg,q): assert arg == 0 self.robot.setConfig(q.tolist()) Jo = self.link.getOrientationJacobian() return _cross_product_twiddle(Jo)
[docs] def gen_derivative(self,arg,q): if len(arg) == 1: return self.derivative(arg[0],q) elif len(arg) == 2: self.robot.setConfig(q.tolist()) Hx,Hy,Hz = self.link.getOrientationHessian() return _cross_product_twiddle([Hx,Hy,Hz]) else: raise NotImplementedError()
[docs] def n_args(self): return 1
[docs] def n_out(self): return 12
[docs] def eval(self,q): self.robot.setConfig(q.tolist()) T = self.link.getTransform() if self.localPos is not None: T = (T[0],vectorops.add(so3.apply(T[0],self.localPos),T[1])) return np.array(T[0]+T[1])
[docs] def derivative(self,arg,q): assert arg == 0 self.robot.setConfig(q.tolist()) J = self.link.getJacobian([0]*3 if self.localPos is None else self.localPos) return np.vstack([_cross_product_twiddle(J[:3])]+[J[3:]])
[docs] def gen_derivative(self,arg,q): if len(arg) == 1: return self.derivative(arg[0],q) elif len(arg) == 2: self.robot.setConfig(q.tolist()) Hx,Hy,Hz = self.link.getPositionHessian([0]*3 if self.localPos is None else self.localPos) Hox,Hoy,Hoz = self.link.getOrientationHessian() Hox = np.array(Hox) Hoy = np.array(Hoy) Hoz = np.array(Hoz) return np.vstack([_cross_product_twiddle([Hox,Hoy,Hoz])]+[[Hx,Hy,Hz]]) else: raise NotImplementedError()
[docs] def n_args(self): return 2
[docs] def n_out(self): return 3
[docs] def eval(self,q,dq): self.robot.setConfig(q.tolist()) self.robot.setVelocity(dq.tolist()) return np.array(self.link.getPointVelocity(self.localPos))
[docs] def derivative(self,arg,q,dq): if arg == 1: self.robot.setConfig(q.tolist()) return np.array(self.link.getPositionJacobian(self.localPos)) else: self.robot.setVelocity(dq.tolist()) Hx,Hy,Hz = self.link.getPositionHessian(self.localPos) return np.row_stack([np.dot(Hx,dq),np.dot(Hy,dq),np.dot(Hz,dq)])
[docs] def n_args(self): return 2
[docs] def n_out(self): return 3
[docs] def eval(self,q,dq): self.robot.setConfig(q.tolist()) self.robot.setVelocity(dq.tolist()) return np.array(self.link.getAngularVelocity())
[docs] def derivative(self,arg,q,dq): if arg == 1: self.robot.setConfig(q.tolist()) return np.array(self.link.getOrientationJacobian()) else: self.robot.setVelocity(dq.tolist()) Hx,Hy,Hz = self.link.getOrientationHessian() return np.row_stack([np.dot(Hx,dq),np.dot(Hy,dq),np.dot(Hz,dq)])
[docs]class LinksToDrivers(ADFunctionInterface): """Autodiff function to convert link values to driver values.""" def __init__(self,robot): self.robot = robot self.drivers = [robot.driver(i) for i in robot.numDrivers()] def __str__(self): return "kinematics.LinksToDrivers[%s]"%(self.robot.getName(),)
[docs] def n_args(self): return 1
[docs] def n_out(self): return self.robot.numDrivers()
[docs] def eval(self,q): self.robot.setConfig(q) return np.array([driver.getValue() for driver in self.drivers])
[docs] def jvp(self,arg,dq,q): self.robot.setConfig(q) self.robot.setVelocity(dq) return np.array([driver.getVelocity() for driver in self.drivers])
[docs]class LinkDerivsToDrivers(ADFunctionInterface): """Autodiff function to convert link velocities to driver velocities.""" def __init__(self,robot): self.robot = robot self.drivers = [robot.driver(i) for i in robot.numDrivers()] def __str__(self): return "kinematics.LinkDerivsToDrivers[%s]"%(self.robot.getName(),)
[docs] def n_args(self): return 1
[docs] def n_out(self): return self.robot.numDrivers()
[docs] def eval(self,v): self.robot.setVelocity(v) return np.array([driver.getVelocity() for driver in self.drivers])
[docs] def jvp(self,arg,dv,v): self.robot.setVelocity(dv) return np.array([driver.getVelocity() for driver in self.drivers])
[docs]class ConfigInterpolate(ADFunctionInterface): """Autodiff wrapper of the RobotModel.interpolate function""" def __init__(self,robot): self.robot = robot def __str__(self): return "kinematics.ConfigInterpolate[%s]"%(self.robot.getName(),)
[docs] def n_args(self): return 3
[docs] def n_in(self,arg): if arg <= 1: return self.robot.numLinks() return 1