def J(self, mb, mbc): X_O_p = self.X_O_p(mbc) # set transformation in Origin orientation frame X_O_p_O = sva.PTransformd(X_O_p.rotation()).inv()*X_O_p jac_mat_dense = self.jac.jacobian(mb, mbc, X_O_p_O) self.jac.fullJacobian(mb, jac_mat_dense, self.jac_mat_sparse) return e.toNumpy(self.jac_mat_sparse)
def J(self, mb, mbc):
X_O_p = self.X_O_p(mbc)
# set transformation in Origin orientation frame
X_O_p_O = sva.PTransformd(X_O_p.rotation()).inv() * X_O_p
jac_mat_dense = self.jac.jacobian(mb, mbc, X_O_p_O)
self.jac.fullJacobian(mb, jac_mat_dense, self.jac_mat_sparse)
return e.toNumpy(self.jac_mat_sparse)
def prismaticJoint(joint): """ Return a prism and the appropriate static transform. """ axis = e.toEigen(e.toNumpy(joint.motionSubspace())[3:]) s = tvtk.CubeSource(x_length=0.02, y_length=0.1, z_length=0.02) quat = e.Quaterniond() quat.setFromTwoVectors(axis, e.Vector3d.UnitY()) return makeActor(s, tuple(axis)), sva.PTransformd(quat)
def revoluteJoint(joint): """ Return a cylinder and the appropriate static transform. """ axis = e.toEigen(e.toNumpy(joint.motionSubspace())[:3]) s = tvtk.CylinderSource(height=0.1, radius=0.02) quat = e.Quaterniond() # Cylinder is around the Y axis quat.setFromTwoVectors(axis, e.Vector3d.UnitY()) return makeActor(s, tuple(axis)), sva.PTransformd(quat)
def g(self, mb, mbc):
q = map(list, mbc.q)
jointConfig = list(mbc.jointConfig)
posInG = 0
for jIndex, j in zip(self.jointIndex, self.joints):
if j.type() in (rbd.Joint.Prism, rbd.Joint.Rev):
self.g_mat[posInG:posInG+j.dof(),0] = q[jIndex][0] - self.q_T[jIndex][0]
elif j.type() in (rbd.Joint.Spherical,):
orid = e.Quaterniond(*self.q_T[jIndex]).inverse().matrix()
self.g_mat[posInG:posInG+j.dof(),0] =\
e.toNumpy(sva.rotationError(orid, jointConfig[jIndex].rotation()))
posInG += j.dof()
return self.g_mat
def g(self, mb, mbc):
q = map(list, mbc.q)
jointConfig = list(mbc.jointConfig)
posInG = 0
for jIndex, j in zip(self.jointIndex, self.joints):
if j.type() in (rbd.Joint.Prism, rbd.Joint.Rev):
self.g_mat[posInG:posInG + j.dof(),
0] = q[jIndex][0] - self.q_T[jIndex][0]
elif j.type() in (rbd.Joint.Spherical, ):
orid = e.Quaterniond(*self.q_T[jIndex]).inverse().matrix()
self.g_mat[posInG:posInG+j.dof(),0] =\
e.toNumpy(sva.rotationError(orid, jointConfig[jIndex].rotation()))
posInG += j.dof()
return self.g_mat
def __init__(self, name, frame_id, X_offset, text_offset, scale, axes_list=[0,1]):
self.name = name
self.scale = scale
self.force_list = [0.] * len(axes_list)
self.axes_list = axes_list
# interactive marker
self.marker = createBoxMarker(scale=0.001)
intMarker = createVisInteractive(frame_id, name, name+' 2dcontrol', X_offset, self.marker)
frame = toNumpy(Matrix3d.Identity())
for i in axes_list:
control = createTranslationControlMarker('control'+str(i), frame[i,:].tolist()[0])
intMarker.controls.append(control)
self.marker_server = InteractiveMarkerServer(name + 'server')
self.marker_server.insert(intMarker, self.markerMoved)
self.marker_server.applyChanges()
# text display of forces
self.text_marker = TextMarker(name, frame_id, text_offset)
self.updateText()
def multiTaskIk(mb, mbc, tasks, delta=1., maxIter=100, prec=1e-8):
"""
The multiTaskIk function is a generator that will return at each call
the new step in the IK process.
Parameters:
- mb: MultiBody system.
- mbc: Initial configuration
- tasks: List of tasks could be of two form:
- (weight, task): apply a global weight on all task dimension
- ((weight,), task): apply a different weight on each dimension of the
task
- delta: Integration step
- maxIter: maximum number of iteration
- prec: stop the IK if \| \alpha \|_{\inf} < prec
Returns:
- Current iteration number
- Current articular position vector q
- Current articular velocity vector alpha (descent direction)
- \| \alpha \|_{\inf}
"""
q = e.toNumpy(rbd.paramToVector(mb, mbc.q))
iterate = 0
minimizer = False
# transform user weight into a numpy array
tasks_np = []
for w, t in tasks:
dim = t.dimension()
w_np = np.zeros((dim,1))
if isinstance(w, (float, int)):
w_np[:,0] = [w]*dim
elif hasattr(w, '__iter__'):
w_np[:,0] = w
else:
raise RuntimeError('%s unknow type for weight vector')
tasks_np.append((w_np, t))
while iterate < maxIter and not minimizer:
# compute task data
gList = map(lambda (w, t): np.mat(w*np.array(t.g(mb, mbc))), tasks_np)
JList = map(lambda (w, t): np.mat(w*np.array(t.J(mb, mbc))), tasks_np)
g = np.concatenate(gList)
J = np.concatenate(JList)
# compute alpha
alpha = -np.mat(np.linalg.lstsq(J, g)[0])
# integrate and run the forward kinematic
mbc.alpha = rbd.vectorToDof(mb, e.toEigenX(alpha))
rbd.eulerIntegration(mb, mbc, delta)
rbd.forwardKinematics(mb, mbc)
# take the new q vector
q = e.toNumpy(rbd.paramToVector(mb, mbc.q))
alphaInf = np.linalg.norm(alpha, np.inf)
yield iterate, q, alpha, alphaInf # yield the current state
# check if the current alpha is a minimizer
if alphaInf < prec:
minimizer = True
iterate += 1
def g(self, mb, mbc): X_O_p = self.X_O_p(mbc) g_body = sva.transformError(self.X_O_T, X_O_p) return e.toNumpy(g_body.vector())
def J(self, mb, mbc): return e.toNumpy(self.comJac.jacobian(mb, mbc))
def g(self, mb, mbc): return e.toNumpy(rbd.computeCoM(mb, mbc) - self.com_T)
def g(self, mb, mbc):
X_O_p = self.X_O_p(mbc)
g_body = sva.transformError(self.X_O_T, X_O_p)
return e.toNumpy(g_body.vector())
def J(self, mb, mbc):
return e.toNumpy(self.comJac.jacobian(mb, mbc))
def g(self, mb, mbc):
return e.toNumpy(rbd.computeCoM(mb, mbc) - self.com_T)
def multiTaskIk(mb, mbc, tasks, delta=1., maxIter=100, prec=1e-8):
"""
The multiTaskIk function is a generator that will return at each call
the new step in the IK process.
Parameters:
- mb: MultiBody system.
- mbc: Initial configuration
- tasks: List of tasks could be of two form:
- (weight, task): apply a global weight on all task dimension
- ((weight,), task): apply a different weight on each dimension of the
task
- delta: Integration step
- maxIter: maximum number of iteration
- prec: stop the IK if \| \alpha \|_{\inf} < prec
Returns:
- Current iteration number
- Current articular position vector q
- Current articular velocity vector alpha (descent direction)
- \| \alpha \|_{\inf}
"""
q = e.toNumpy(rbd.paramToVector(mb, mbc.q))
iterate = 0
minimizer = False
# transform user weight into a numpy array
tasks_np = []
for w, t in tasks:
dim = t.dimension()
w_np = np.zeros((dim, 1))
if isinstance(w, (float, int)):
w_np[:, 0] = [w] * dim
elif hasattr(w, '__iter__'):
w_np[:, 0] = w
else:
raise RuntimeError('%s unknow type for weight vector')
tasks_np.append((w_np, t))
while iterate < maxIter and not minimizer:
# compute task data
gList = map(lambda (w, t): np.mat(w * np.array(t.g(mb, mbc))),
tasks_np)
JList = map(lambda (w, t): np.mat(w * np.array(t.J(mb, mbc))),
tasks_np)
g = np.concatenate(gList)
J = np.concatenate(JList)
# compute alpha
alpha = -np.mat(np.linalg.lstsq(J, g)[0])
# integrate and run the forward kinematic
mbc.alpha = rbd.vectorToDof(mb, e.toEigenX(alpha))
rbd.eulerIntegration(mb, mbc, delta)
rbd.forwardKinematics(mb, mbc)
# take the new q vector
q = e.toNumpy(rbd.paramToVector(mb, mbc.q))
alphaInf = np.linalg.norm(alpha, np.inf)
yield iterate, q, alpha, alphaInf # yield the current state
# check if the current alpha is a minimizer
if alphaInf < prec:
minimizer = True
iterate += 1
def publish(self): self.ros_publisher.publish(toNumpy(self.task.eval()))