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 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()))