def cost(self, x): tauq, fr, fl = self.x2vars(x) pinocchio.computeAllTerms(self.rmodel, self.rdata, self.q, self.vq) b = self.rdata.nle pinocchio.updateFramePlacements(self.rmodel, self.rdata) Jr = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idR, LOCAL) Jl = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idL, LOCAL) self.residuals = m2a(b - tauq - Jr.T * fr - Jl.T * fl) return sum(self.residuals**2)
def calcDiff(self, data, x, recalc=True): if recalc: self.calc(data, x) dv_dq, da_dq, da_dv, da_da = pinocchio.getJointAccelerationDerivatives( self.pinocchio, data.pinocchio, data.joint, pinocchio.ReferenceFrame.LOCAL) dv_dq, dv_dvq = pinocchio.getJointVelocityDerivatives( self.pinocchio, data.pinocchio, data.joint, pinocchio.ReferenceFrame.LOCAL) vw = data.v.angular vv = data.v.linear data.Aq[:, :] = (data.fXj * da_dq)[:3, :] + skew(vw) * ( data.fXj * dv_dq)[:3, :] - skew(vv) * (data.fXj * dv_dq)[3:, :] data.Av[:, :] = (data.fXj * da_dv)[:3, :] + skew(vw) * data.J - skew(vv) * data.Jw R = data.pinocchio.oMf[self.frame].rotation if self.gains[0] != 0.: data.Aq[:, :] += self.gains[0] * R * pinocchio.getFrameJacobian( self.pinocchio, data.pinocchio, self.frame, pinocchio.ReferenceFrame.LOCAL)[:3, :] if self.gains[1] != 0.: data.Aq[:, :] += self.gains[1] * (data.fXj[:3, :] * dv_dq) data.Av[:, :] += self.gains[1] * (data.fXj[:3, :] * dv_dvq)
def computeJacobian(self, q): pin.forwardKinematics(self.rmodel, self.rdata, q) pin.updateFramePlacements(self.rmodel, self.rdata) pin.computeJointJacobians(self.rmodel, self.rdata, q) J = pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) return J
def test_frame_algo(self): model = self.model data = model.createData() q = pin.neutral(model) v = np.random.rand((model.nv)) frame_id = self.frame_idx J1 = pin.computeFrameJacobian(model, data, q, frame_id) J2 = pin.computeFrameJacobian(model, data, q, frame_id, pin.LOCAL) self.assertApprox(J1, J2) data2 = model.createData() pin.computeJointJacobians(model, data2, q) J3 = pin.getFrameJacobian(model, data2, frame_id, pin.LOCAL) self.assertApprox(J1, J3) dJ1 = pin.frameJacobianTimeVariation(model, data, q, v, frame_id, pin.LOCAL) data3 = model.createData() pin.computeJointJacobiansTimeVariation(model, data3, q, v) dJ2 = pin.getFrameJacobianTimeVariation(model, data3, frame_id, pin.LOCAL) self.assertApprox(dJ1, dJ2)
def calcDiff3d(self, q): pin.forwardKinematics(robot.model, robot.data, q) M = pin.updateFramePlacement(robot.model, robot.data, self.frameIndex) pin.computeJointJacobians(robot.model, robot.data, q) J = pin.getFrameJacobian(robot.model, robot.data, self.frameIndex, pin.LOCAL_WORLD_ALIGNED)[:3, :] return 2 * J.T @ (M.translation - self.Mtarget.translation)
def main(): ''' Main procedure 1 ) Build the model from an URDF file 2 ) compute forwardKinematics 3 ) compute forwardKinematics of the frames 4 ) explore data ''' model_file = Path(__file__).absolute().parents[1].joinpath( 'urdf', 'twolinks.urdf') model = buildModelFromUrdf(str(model_file)) # Create data required by the algorithms data = model.createData() # Sample a random configuration numpy_vector_joint_pos = randomConfiguration(model) # THIS FUNCTION CALLS THE FORWARD KINEMATICS computeJointJacobians(model, data, numpy_vector_joint_pos) joint_number = model.njoints for i in range(joint_number): joint = model.joints[i] joint_placement = data.oMi[i] joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.WORLD) # CAUTION updateFramePlacements must be called to update frame's positions # Remark: in pinocchio frames, joints and bodies are different things updateFramePlacements(model, data) # Print out the placement of each joint of the kinematic tree frame_number = model.nframes for i in range(frame_number): frame = model.frames[i] frame_placement = data.oMf[i] frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.WORLD)
def calcDiff(self, data, x): v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da = pinocchio.getJointAccelerationDerivatives( self.state.pinocchio, data.pinocchio, self.joint, pinocchio.ReferenceFrame.LOCAL) vv_skew = pinocchio.utils.skew(self.vv) vw_skew = pinocchio.utils.skew(self.vw) fXjdv_dq = self.fXj * v_partial_dq da0_dq = ( self.fXj * a_partial_dq )[:3, :] + vw_skew * fXjdv_dq[:3, :] - vv_skew * fXjdv_dq[3:, :] da0_dv = (self.fXj * a_partial_dv)[:3, :] + vw_skew * data.Jc - vv_skew * self.Jw if np.asscalar(self.gains[0]) != 0.: R = data.pinocchio.oMf[self.xref.frame].rotation da0_dq += np.asscalar( self.gains[0]) * R * pinocchio.getFrameJacobian( self.state.pinocchio, data.pinocchio, self.xref.frame, pinocchio.ReferenceFrame.LOCAL)[:3, :] if np.asscalar(self.gains[1]) != 0.: da0_dq += np.asscalar( self.gains[1]) * (self.fXj[:3, :] * v_partial_dq) da0_dv += np.asscalar( self.gains[1]) * (self.fXj[:3, :] * a_partial_da) data.da0_dx[:, :] = np.hstack([da0_dq, da0_dv])
def calcDiff(self, data, x, u, recalc=True): if recalc: self.calc(data, x, u) pinocchio.updateFramePlacements(self.state.pinocchio, data.pinocchio) data.rJf = pinocchio.Jlog6(data.rMf) data.fJf = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.Mref.frame, pinocchio.ReferenceFrame.LOCAL) data.J = data.rJf * data.fJf self.activation.calcDiff(data.activation, data.r, recalc) data.Rx = np.hstack([ data.J, pinocchio.utils.zero((self.activation.nr, self.state.nv)) ]) data.Lx = np.vstack([ data.J.T * data.activation.Ar, pinocchio.utils.zero((self.state.nv, 1)) ]) data.Lxx = np.vstack([ np.hstack([ data.J.T * data.activation.Arr * data.J, pinocchio.utils.zero((self.state.nv, self.state.nv)) ]), pinocchio.utils.zero((self.state.nv, self.state.ndx)) ])
def Jtf(q, f): pinocchio.computeJointJacobians(rmodel, rdata, q) pinocchio.forwardKinematics(rmodel, rdata, q) pinocchio.updateFramePlacements(rmodel, rdata) J = pinocchio.getFrameJacobian(rmodel, rdata, CONTACTFRAME, pinocchio.ReferenceFrame.LOCAL) return J.T * f
def _inverse_kinematics_step( self, frame_id: int, xdes: np.ndarray, q0: np.ndarray) -> typing.Tuple[np.ndarray, np.ndarray]: """Compute one IK iteration for a single finger.""" dt = 1.0e-1 pinocchio.computeJointJacobians( self.robot_model, self.data, q0, ) pinocchio.framesForwardKinematics( self.robot_model, self.data, q0, ) Ji = pinocchio.getFrameJacobian( self.robot_model, self.data, frame_id, pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED, )[:3, :] xcurrent = self.data.oMf[frame_id].translation try: Jinv = np.linalg.inv(Ji) except Exception: Jinv = np.linalg.pinv(Ji) err = xdes - xcurrent dq = Jinv.dot(xdes - xcurrent) qnext = pinocchio.integrate(self.robot_model, q0, dt * dq) return qnext, err
def calcDiff(self, q): pin.framesForwardKinematics(self.rmodel, self.rdata, q) pin.computeJointJacobians(self.rmodel, self.rdata, q) M = self.rdata.oMf[self.frameIndex] J = pin.getFrameJacobian(self.rmodel, self.rdata, self.frameIndex, pin.LOCAL_WORLD_ALIGNED)[:3, :] return 2 * J.T @ (M.translation - self.ptarget)
def get_world_oriented_frame_jacobian(self, index): self.robot.forwardKinematics(self.q, self.dq) self.robot.computeJointJacobians(self.q) self.robot.framesForwardKinematics(self.q) jac = pinocchio.getFrameJacobian(self.model, self.data, index, pinocchio.ReferenceFrame.LOCAL) world_R_joint = pinocchio.SE3(self.data.oMf[index].rotation, zero(3)) return world_R_joint.action.dot(jac)
def MvJtf(q, vnext, v, f): M = pinocchio.crba(rmodel, rdata, q) pinocchio.computeJointJacobians(rmodel, rdata, q) pinocchio.forwardKinematics(rmodel, rdata, q) pinocchio.updateFramePlacements(rmodel, rdata) J = pinocchio.getFrameJacobian(rmodel, rdata, CONTACTFRAME, pinocchio.ReferenceFrame.LOCAL) return M * (vnext - v) - J.T * f
def computeJacobian(self, q): pin.forwardKinematics(self.rmodel, self.rdata, q) pin.updateFramePlacements(self.rmodel, self.rdata) pin.computeJointJacobians(self.rmodel, self.rdata, q) J = pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) #modify J to remove the term corresponding to the base frame orientation J = np.hstack([J[:, :3], np.zeros((6, 4)), J[:, 6:]]) return J
def main(): ''' Main procedure 1 ) Build the model from an URDF file 2 ) compute forwardKinematics 3 ) compute forwardKinematics of the frames 4 ) explore data ''' model_file = Path(__file__).absolute().parents[1].joinpath( 'urdf', 'twolinks.urdf') model = buildModelFromUrdf(str(model_file)) # Create data required by the algorithms data = model.createData() # Sample a random configuration numpy_vector_joint_pos = randomConfiguration(model) numpy_vector_joint_vel = np.random.rand(model.njoints - 1) # Perform the forward kinematics over the kinematic tree forwardKinematics(model, data, numpy_vector_joint_pos, numpy_vector_joint_vel) computeJointJacobians(model, data, numpy_vector_joint_pos) joint_number = model.njoints for i in range(joint_number): joint = model.joints[i] joint_placement = data.oMi[i] joint_velocity = getVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) joint_linear_vel = joint_velocity.linear joint_angular_vel = joint_velocity.angular joint_6v_vel = joint_velocity.vector err = joint_6v_vel - joint_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err # CAUTION updateFramePlacements must be called to update frame's positions # Remark: in pinocchio frames, joints and bodies are different things updateFramePlacements(model, data) frame_number = model.nframes for i in range(frame_number): frame = model.frames[i] frame_placement = data.oMf[i] frame_velocity = getFrameVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) frame_linear_vel = frame_velocity.linear frame_angular_vel = frame_velocity.angular frame_6v_vel = frame_velocity.vector err = frame_6v_vel - frame_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def calcDiff(self, q, recalc=False): if recalc: self.calc(q) J = np.dot( pin.Jlog3(self.rMf), pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id, pin.ReferenceFrame.LOCAL)[3:, :]) self.J = self.weight_matrix.dot(J) return self.J
def dConstraint_dx(self, x): q = a2m(x) pinocchio.forwardKinematics(self.rmodel, self.rdata, q) pinocchio.computeJointJacobians(self.rmodel, self.rdata, q) pinocchio.updateFramePlacements(self.rmodel, self.rdata) refMeff = self.refEff.inverse() * self.rdata.oMf[self.idEff] log_M = pinocchio.Jlog6(refMeff) M_q = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idEff, LOCAL) self.Jeq = log_M * M_q return self.Jeq
def calc(self, data, x): assert (self.Mref.oMf is not None or self.gains[0] == 0.) data.Jc = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.Mref.frame, pinocchio.ReferenceFrame.LOCAL) data.a0 = pinocchio.getFrameAcceleration(self.state.pinocchio, data.pinocchio, self.Mref.frame).vector if self.gains[0] != 0.: self.rMf = self.Mref.oMf.inverse() * data.pinocchio.oMf[self.Mref.frame] data.a0 += np.asscalar(self.gains[0]) * pinocchio.log6(self.rMf).vector if self.gains[1] != 0.: v = pinocchio.getFrameVelocity(self.state.pinocchio, data.pinocchio, self.Mref.frame).vector data.a0 += np.asscalar(self.gains[1]) * v
def get_link_jacobian(self, link_id): frame_id = self._model.getFrameId(link_id) pin.computeJointJacobians(self._model, self._data, self._q) jac = pin.getFrameJacobian(self._model, self._data, frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) # Pinocchio has linear on top of angular ret = np.zeros_like(jac) ret[0:3] = jac[3:6] ret[3:6] = jac[0:3] return np.copy(ret)
def getJacobian(self, q=None): if q is None: q = self.current_j_pos if q.shape[0] == 7: q_tmp = q.copy() q = np.zeros(9) q[:7] = q_tmp pinocchio.computeJointJacobians(self.model, self.data, q) pinocchio.framesForwardKinematics(self.model, self.data, q) return pinocchio.getFrameJacobian(self.model, self.data, self.end_effector_frame_id, pinocchio.LOCAL_WORLD_ALIGNED)[:, :7]
def calcDiff(self, q, recalc=False): if recalc: self.calc(q) R = self.rdata.oMf[self.ee_frame_id].rotation J = R.dot( pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id, pin.ReferenceFrame.LOCAL)[:3, :]) self.J = self.weight_matrix.dot(J) return self.J
def main(): ''' Main procedure 1 ) Build the model from an URDF file 2 ) compute forwardKinematics 3 ) compute forwardKinematics of the frames 4 ) explore data ''' model_file = Path(__file__).absolute().parents[1].joinpath( 'urdf', 'twolinks.urdf') model = buildModelFromUrdf(str(model_file)) # Create data required by the algorithms data = model.createData() # Sample a random configuration numpy_vector_joint_pos = randomConfiguration(model) numpy_vector_joint_vel = np.random.rand(model.njoints - 1) numpy_vector_joint_acc = np.random.rand(model.njoints - 1) # Calls Reverese Newton-Euler algorithm numpy_vector_joint_torques = rnea(model, data, numpy_vector_joint_pos, numpy_vector_joint_vel, numpy_vector_joint_acc) # IN WHAT FOLLOWS WE CONFIRM THAT rnea COMPUTES THE FORWARD KINEMATCS computeJointJacobians(model, data, numpy_vector_joint_pos) joint_number = model.njoints for i in range(joint_number): joint = model.joints[i] joint_placement = data.oMi[i] joint_velocity = getVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) err = joint_velocity.vector - \ joint_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err # CAUTION updateFramePlacements must be called to update frame's positions # Remark: in pinocchio frames, joints and bodies are different things updateFramePlacements(model, data) frame_number = model.nframes for i in range(frame_number): frame = model.frames[i] frame_placement = data.oMf[i] frame_velocity = getFrameVelocity(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.LOCAL_WORLD_ALIGNED) err = frame_velocity.vector - \ frame_jacobian.dot(numpy_vector_joint_vel) assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def calcDiff(self, data, x, u): pinocchio.updateFramePlacements(self.state.pinocchio, data.shared.pinocchio) data.R = data.shared.pinocchio.oMf[self.xref.frame].rotation data.J = data.R * pinocchio.getFrameJacobian(self.state.pinocchio, data.shared.pinocchio, self.xref.frame, pinocchio.ReferenceFrame.LOCAL)[:3, :] self.activation.calcDiff(data.activation, data.r) data.Rx = np.hstack([data.J, pinocchio.utils.zero((self.activation.nr, self.state.nv))]) data.Lx = np.vstack([data.J.T * data.activation.Ar, pinocchio.utils.zero((self.state.nv, 1))]) data.Lxx = np.vstack([ np.hstack([data.J.T * data.activation.Arr * data.J, pinocchio.utils.zero((self.state.nv, self.state.nv))]), pinocchio.utils.zero((self.state.nv, self.state.ndx)) ])
def dConstraint_dx_rightfoot(self, x, nc=0): q = self.vq2q(a2m(x)) pinocchio.forwardKinematics(self.rmodel, self.rdata, q) pinocchio.updateFramePlacements(self.rmodel, self.rdata) pinocchio.computeJointJacobians(self.rmodel, self.rdata, q) pinocchio.updateFramePlacements(self.rmodel, self.rdata) refMr = self.refR.inverse() * self.rdata.oMf[self.idR] log_M = pinocchio.Jlog6(refMr) M_q = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idR, LOCAL) Q_v = self.dq_dv(a2m(x)) self.Jeq[nc:nc + 6, :] = log_M * M_q * Q_v return self.Jeq[nc:nc + 6, :]
def calcDiff(self, data, x, u): pinocchio.updateFramePlacements(self.state.pinocchio, data.shared.pinocchio) data.rJf = pinocchio.Jlog3(data.rRf) data.fJf = pinocchio.getFrameJacobian(self.state.pinocchio, data.shared.pinocchio, self.Rref.frame, pinocchio.ReferenceFrame.LOCAL)[3:, :] data.J = data.rJf * data.fJf self.activation.calcDiff(data.activation, data.r) data.Rx = np.hstack([data.J, pinocchio.utils.zero((self.activation.nr, self.state.nv))]) data.Lx = np.vstack([data.J.T * data.activation.Ar, pinocchio.utils.zero((self.state.nv, 1))]) data.Lxx = np.vstack([ np.hstack([data.J.T * data.activation.Arr * data.J, pinocchio.utils.zero((self.state.nv, self.state.nv))]), pinocchio.utils.zero((self.state.nv, self.state.ndx)) ])
def Jacobian(self, q): """ Description: 1. Computes the Jacobian Tensor of the robot. Args: q -> np.array (3,) : Joint Positions of the robot. Returns: np.array (3,3): Jacobian Tensor of the robot. """ frame = pin.ReferenceFrame.LOCAL_WORLD_ALIGNED pin.computeJointJacobians(self.model, self.model_data, q) return pin.getFrameJacobian(self.model, self.model_data, self.EOAT_ID, frame)[:3]
def calc(self, data, x): # We suppose forwardKinematics(q,v,a), computeJointJacobian and updateFramePlacement already # computed. assert (self.ref is not None or self.gains[0] == 0.) data.J[:, :] = pinocchio.getFrameJacobian( self.pinocchio, data.pinocchio, self.frame, pinocchio.ReferenceFrame.LOCAL) data.a0[:] = pinocchio.getFrameAcceleration(self.pinocchio, data.pinocchio, self.frame).vector.flat if self.gains[0] != 0.: data.rMf = self.ref.inverse() * data.pinocchio.oMf[self.frame] data.a0[:] += self.gains[0] * m2a(pinocchio.log(data.rMf).vector) if self.gains[1] != 0.: data.a0[:] += self.gains[1] * m2a( pinocchio.getFrameVelocity(self.pinocchio, data.pinocchio, self.frame).vector)
def calcDiff(self, data, x, u, recalc=True): if recalc: self.calc(data, x, u) nq = self.nq pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio) R = data.pinocchio.oMf[self.frame].rotation J = R * pinocchio.getFrameJacobian( self.pinocchio, data.pinocchio, self.frame, pinocchio.ReferenceFrame.LOCAL)[:3, :] Ax, Axx = self.activation.calcDiff(data.activation, data.residuals, recalc=recalc) data.Rq[:, :nq] = J data.Lq[:] = np.dot(J.T, Ax) data.Lqq[:, :] = np.dot(data.Rq.T, Axx * data.Rq) # J is a matrix, use Rq instead. return data.cost
def calc(self, data, x): assert (self.xref.oxf is not None or self.gains[0] == 0.) v = pinocchio.getFrameVelocity(self.state.pinocchio, data.pinocchio, self.xref.frame) self.vw = v.angular self.vv = v.linear fJf = pinocchio.getFrameJacobian(self.state.pinocchio, data.pinocchio, self.xref.frame, pinocchio.ReferenceFrame.LOCAL) data.Jc = fJf[:3, :] self.Jw = fJf[3:, :] data.a0 = pinocchio.getFrameAcceleration(self.state.pinocchio, data.pinocchio, self.xref.frame).linear + pinocchio.utils.cross(self.vw, self.vv) if self.gains[0] != 0.: data.a0 += np.asscalar(self.gains[0]) * (data.pinocchio.oMf[self.xref.frame].translation - self.xref.oxf) if self.gains[1] != 0.: data.a0 += np.asscalar(self.gains[1]) * self.vv
def check_offset_to_pinocchio_model_in_cart_space(self, q): import pinocchio from classic_framework.utils.geometric_transformation import mat2quat obj_urdf = sim_framework_path("./envs/panda_arm_hand_pinocchio.urdf") model = pinocchio.buildModelFromUrdf(obj_urdf) data = model.createData() q_pin = np.zeros(9) q_pin[:7] = q pinocchio.framesForwardKinematics(model, data, q_pin) pinocchio.computeJointJacobians(model, data, q_pin) pinocchio.framesForwardKinematics(model, data, q_pin) # get orientation rotation_matrix = data.oMf[model.getFrameId('panda_grasptarget')].rotation quaternions = mat2quat(rotation_matrix) # [w x y z] jac = pinocchio.getFrameJacobian(model, data, model.getFrameId('panda_grasptarget'), pinocchio.LOCAL_WORLD_ALIGNED)[:, :7] print('position offset:', self.current_c_pos - np.array(data.oMf[model.getFrameId('panda_grasptarget')].translation))