def setUp(self): self.x = self.ROBOT_STATE.rand() self.robot_data = self.ROBOT_MODEL.createData() self.data = self.IMPULSE.createData(self.robot_data) self.data_der = self.IMPULSE_DER.createData(self.robot_data) nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:], pinocchio.utils.zero(nv)) pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data) pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data) pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:], pinocchio.utils.zero(nv))
def calcDiff(self, q): Jp = pinv( pin.computeJointJacobian(robot.model, robot.data, q, self.jointIndex)) res = np.zeros(robot.model.nv) v0 = np.zeros(robot.model.nv) for k in range(6): pin.computeForwardKinematicsDerivatives(robot.model, robot.data, q, Jp[:, k], v0) JqJpk = pin.getJointVelocityDerivatives(robot.model, robot.data, self.jointIndex, pin.LOCAL)[0] res += JqJpk[k, :] res *= self.calc(q) return res
def setUp(self): self.robot_data = self.ROBOT_MODEL.createData() self.x = self.ROBOT_STATE.rand() self.u = pinocchio.utils.rand(self.ROBOT_MODEL.nv) self.multibody_data = crocoddyl.DataCollectorMultibody(self.robot_data) self.data = self.COST.createData(self.multibody_data) self.data_der = self.COST_DER.createData(self.multibody_data) nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:]) pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:], pinocchio.utils.zero(nv)) pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data, self.x[:nq]) pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data) pinocchio.jacobianCenterOfMass(self.ROBOT_MODEL, self.robot_data, self.x[:nq], False)
def test_numdiff(self): rmodel,rdata = self.rmodel,self.rdata q,vq,aq,dq= self.q,self.vq,self.aq,self.dq #### Compute d/dq VCOM with the algo. pin.computeAllTerms(rmodel,rdata,q,vq) pin.computeForwardKinematicsDerivatives(rmodel,rdata,q,vq,aq) dvc_dq = pin.getCenterOfMassVelocityDerivatives(rmodel,rdata) #### Approximate d/dq VCOM by finite diff. def calc_vc(q,vq): """ Compute COM velocity """ pin.centerOfMass(rmodel,rdata,q,vq) return rdata.vcom[0] dvc_dqn = df_dq(rmodel,lambda _q: calc_vc(_q,vq),q) self.assertTrue(np.allclose(dvc_dq,dvc_dqn,atol=np.sqrt(self.precision)))
def setUp(self): self.x = self.ROBOT_STATE.rand() self.robot_data = self.ROBOT_MODEL.createData() self.contacts = crocoddyl.ContactModelMultiple(self.ROBOT_STATE) self.contacts.addContact("myContact", self.CONTACT) self.data = self.CONTACT.createData(self.robot_data) self.data_multiple = self.contacts.createData(self.robot_data) nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:], pinocchio.utils.zero(nv)) pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data) pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data) pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:], pinocchio.utils.zero(nv))
def setUp(self): self.x = self.ROBOT_STATE.rand() self.robot_data = self.ROBOT_MODEL.createData() self.impulseSum = crocoddyl.ImpulseModelMultiple(self.ROBOT_STATE) self.datas = collections.OrderedDict([[name, impulse.createData(self.robot_data)] for name, impulse in self.IMPULSES.items()]) for name, impulse in self.IMPULSES.items(): self.impulseSum.addImpulse(name, impulse) self.dataSum = self.impulseSum.createData(self.robot_data) nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:], pinocchio.utils.zero(nv)) pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data) pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data) pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:], pinocchio.utils.zero(nv))
def test_numdiff(self): rmodel, rdata = self.rmodel, self.rdata q, vq, aq, dq = self.q, self.vq, self.aq, self.dq #### Compute d/dq VCOM with the algo. pin.computeAllTerms(rmodel, rdata, q, vq) pin.computeForwardKinematicsDerivatives(rmodel, rdata, q, vq, aq) dvc_dq = pin.getCenterOfMassVelocityDerivatives(rmodel, rdata) #### Approximate d/dq VCOM by finite diff. def calc_vc(q, vq): """ Compute COM velocity """ pin.centerOfMass(rmodel, rdata, q, vq) return rdata.vcom[0].copy() dvc_dqn = df_dq(rmodel, lambda _q: calc_vc(_q, vq), q) self.assertTrue( np.allclose(dvc_dq, dvc_dqn, atol=np.sqrt(self.precision)))
def test_derivatives(self): model = self.model data = self.data q = self.q v = self.v a = self.a pin.computeForwardKinematicsDerivatives(model, data, q, v, a) pin.getFrameVelocityDerivatives(model, data, self.frame_idx, pin.WORLD) pin.getFrameVelocityDerivatives(model, data, self.frame_idx, pin.LOCAL) pin.getFrameVelocityDerivatives(model, data, self.frame_idx, pin.LOCAL_WORLD_ALIGNED) pin.getFrameAccelerationDerivatives(model, data, self.frame_idx, pin.WORLD) pin.getFrameAccelerationDerivatives(model, data, self.frame_idx, pin.LOCAL) pin.getFrameAccelerationDerivatives(model, data, self.frame_idx, pin.LOCAL_WORLD_ALIGNED)
def setUp(self): self.robot_data = self.ROBOT_MODEL.createData() self.x = self.ROBOT_STATE.rand() self.u = pinocchio.utils.rand(self.ROBOT_MODEL.nv) self.cost_sum = crocoddyl.CostModelSum(self.ROBOT_STATE) self.cost_sum.addCost('myCost', self.COST, 1.) self.data = self.COST.createData(self.robot_data) self.data_sum = self.cost_sum.createData(self.robot_data) nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:]) pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:], pinocchio.utils.zero(nv)) pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data, self.x[:nq]) pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data) pinocchio.jacobianCenterOfMass(self.ROBOT_MODEL, self.robot_data, self.x[:nq], False)
model = pin.buildSampleModelHumanoidRandom() data = model.createData() # Set bounds (by default they are undefinded) model.lowerPositionLimit = -np.matrix(np.ones((model.nq, 1))) model.upperPositionLimit = np.matrix(np.ones((model.nq, 1))) q = pin.randomConfiguration(model) # joint configuration v = np.matrix(np.random.rand(model.nv, 1)) # joint velocity a = np.matrix(np.random.rand(model.nv, 1)) # joint acceleration # Evaluate all the terms required by the kinematics derivatives pin.computeForwardKinematicsDerivatives(model, data, q, v, a) # Evaluate the derivatives for a precise joint (e.g. rleg6_joint) joint_name = "rleg6_joint" joint_id = model.getJointId(joint_name) # Derivatives of the spatial velocity with respect to the joint configuration and velocity vectors (dv_dq, dv_dv) = pin.getJointVelocityDerivatives(model, data, joint_id, pin.ReferenceFrame.WORLD) # or to get them in the LOCAL frame of the joint (dv_dq_local, dv_dv_local) = pin.getJointVelocityDerivatives(model, data, joint_id, pin.ReferenceFrame.LOCAL)
def calcwxv(q, vq, aq): pinocchio.forwardKinematics(model, data, q, vq, aq) return cross(data.v[-1].angular, data.v[-1].linear) daa_dqn = df_dq(model, partial(calcaa, vq=vq, aq=aq), q) da_dqn = df_dq(model, partial(calca, vq=vq, aq=aq), q) dwxv_dqn = df_dq(model, partial(calcwxv, vq=vq, aq=aq), q) pinocchio.computeJointJacobians(model, data, q) J = pinocchio.getJointJacobian(model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL) Jv = J[:3, :] Jw = J[3:, :] # a + wxv pinocchio.computeForwardKinematicsDerivatives(model, data, q, vq, aq) # da_dq = data.ddq_dq dv_dq, da_dq, da_dv, da_da = pinocchio.getJointAccelerationDerivatives( model, data, model.joints[-1].id, pinocchio.ReferenceFrame.LOCAL) assertNumDiff( da_dq, da_dqn, NUMDIFF_MODIFIER * h) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__) def calcv(q, vq, aq): pinocchio.forwardKinematics(model, data, q, vq, aq) return data.v[-1].linear def calcw(q, vq, aq): pinocchio.forwardKinematics(model, data, q, vq, aq)
costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(costData.Lxu, costDataND.Lxu, NUMDIFF_MODIFIER * costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(costData.Luu, costDataND.Luu, NUMDIFF_MODIFIER * costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__) q = pinocchio.randomConfiguration(rmodel) v = rand(rmodel.nv) x = m2a(np.concatenate([q, v])) u = m2a(rand(rmodel.nv)) costModel = CostModelFrameVelocity(rmodel, rmodel.getFrameId('gripper_left_fingertip_2_link')) costData = costModel.createData(rdata) pinocchio.forwardKinematics(rmodel, rdata, q, v) pinocchio.computeForwardKinematicsDerivatives(rmodel, rdata, q, v, zero(rmodel.nv)) pinocchio.updateFramePlacements(rmodel, rdata) costModel.calcDiff(costData, x, u) costModelND = CostModelNumDiff( costModel, StatePinocchio(rmodel), withGaussApprox=True, reevals=[ lambda m, d, x, u: pinocchio.forwardKinematics(m, d, a2m(x[:rmodel.nq]), a2m(x[rmodel.nq:])), lambda m, d, x, u: pinocchio.computeForwardKinematicsDerivatives(m, d, a2m(x[:rmodel.nq]), a2m(x[ rmodel.nq:]), zero(rmodel.nv)), lambda m, d, x, u: pinocchio.updateFramePlacements(m, d) ]) costDataND = costModelND.createData(rdata)
contactModel = ContactModel6D( rmodel, rmodel.getFrameId('gripper_left_fingertip_2_link'), ref=pinocchio.SE3.Random(), gains=[4., 4.]) contactData = contactModel.createData(rdata) q = pinocchio.randomConfiguration(rmodel) v = rand(rmodel.nv) x = m2a(np.concatenate([q, v])) u = m2a(rand(rmodel.nv - 6)) pinocchio.forwardKinematics(rmodel, rdata, q, v, zero(rmodel.nv)) pinocchio.computeJointJacobians(rmodel, rdata) pinocchio.updateFramePlacements(rmodel, rdata) pinocchio.computeForwardKinematicsDerivatives(rmodel, rdata, q, v, zero(rmodel.nv)) contactModel.calc(contactData, x) contactModel.calcDiff(contactData, x) rdata2 = rmodel.createData() pinocchio.computeAllTerms(rmodel, rdata2, q, v) pinocchio.updateFramePlacements(rmodel, rdata2) contactData2 = contactModel.createData(rdata2) contactModel.calc(contactData2, x) assert (norm(contactData.a0 - contactData2.a0) < 1e-9) assert (norm(contactData.J - contactData2.J) < 1e-9) def returna_at0(q, v): x = np.vstack([q, v]).flat pinocchio.computeAllTerms(rmodel, rdata2, q, v)
def calcDiff(self, data, x, u=None, recalc=True): ''' k = [Mv;0]; K = [MJ^T;J0] r = [vnext;f] = K^-1 k dr/dv = K^-1 [M;0] dr/dq = -K^-1 K'K^-1 k + K^-1 k' = -K^-1 (K'r-k') = -K^-1 [ M'vnext + J'^T f- M'v ] [ J'vnext ] = -K^-1 [ M'(vnext-v) + J'^T f ] [ J' vnext ] ''' if recalc: xout, cost = self.calc(data, x, u) nq, nv = self.nq, self.nv q = a2m(x[:nq]) v = a2m(x[-nv:]) vnext = a2m(data.vnext) fs = data.impulse.forces # Derivative M' dv + J'f + b' g6bak = self.pinocchio.gravity.copy() self.pinocchio.gravity = pinocchio.Motion.Zero() pinocchio.computeRNEADerivatives(self.pinocchio, data.pinocchio, q, zero(nv), vnext - v, fs) self.pinocchio.gravity = g6bak data.did_dq[:, :] = data.pinocchio.dtau_dq # Derivative of the impulse constraint pinocchio.computeForwardKinematicsDerivatives(self.pinocchio, data.pinocchio, q, vnext, zero(nv)) # pinocchio.updateFramePlacements(self.pinocchio,data.pinocchio) self.impulse.calcDiff(data.impulse, x, recalc=False) data.dv_dq = data.impulse.Vq data.Fq[:nv, :] = 0 np.fill_diagonal(data.Fq[:nv, :], 1) # dq/dq data.Fv[:nv, :] = 0 # dq/dv data.Fx[nv:, :nv] = -np.dot(data.Kinv[:nv, :], np.vstack([data.did_dq, data.dv_dq ])) # dvnext/dq data.Fx[nv:, nv:] = np.dot(data.Kinv[:nv, :nv], data.K[:nv, :nv]) # dvnext/dv # data.Rx[:,:] = 0 # np.fill_diagonal(data.Rv,-1) # data.Rx[:,:] += data.Fx[nv:,:] # data.Rx *= self.impulseWeight # data.Lx [:] = np.dot(data.Rx.T,data.costResiduals) # data.Lxx[:,:] = np.dot(data.Rx.T,data.Rx) if isinstance(self.costs, CostModelImpactBase): self.costs.setImpactDiffData(data.costs, data.Fx[nv:, :]) if isinstance(self.costs, CostModelSum): for cmodel, cdata in zip(self.costs.costs.values(), data.costs.costs.values()): if isinstance(cmodel.cost, CostModelImpactBase): cmodel.cost.setImpactDiffData(cdata, data.Fx[nv:, :]) self.costs.calcDiff(data.costs, x, u=None, recalc=recalc) return data.xnext, data.cost