def calcDiff(self, data, x, u=None): self.costsData.shareMemory(data) nq, nv = self.state.nq, self.state.nv q, v = x[:nq], x[-nv:] if u is None: u = self.unone if True: self.calc(data, x, u) self.actuation.calcDiff(self.actuationData, x, u) tau = self.actuationData.tau # Computing the dynamics derivatives if self.enable_force: pinocchio.computeABADerivatives(self.state.pinocchio, self.pinocchioData, q, v, tau) ddq_dq = self.pinocchioData.ddq_dq ddq_dv = self.pinocchioData.ddq_dv data.Fx[:, :] = np.hstack([ ddq_dq, ddq_dv ]) + self.pinocchioData.Minv * self.actuationData.dtau_dx data.Fu[:, :] = self.pinocchioData.Minv * self.actuationData.dtau_du else: pinocchio.computeRNEADerivatives(self.state.pinocchio, self.pinocchioData, q, v, data.xout) ddq_dq = self.Minv * (self.actuationData.dtau_dx[:, :nv] - self.pinocchioData.dtau_dq) ddq_dv = self.Minv * (self.actuationData.dtau_dx[:, nv:] - self.pinocchioData.dtau_dv) data.Fx[:, :] = np.hstack([ddq_dq, ddq_dv]) data.Fu[:, :] = self.Minv * self.actuationData.dtau_du # Computing the cost derivatives self.costs.calcDiff(self.costsData, x, u)
def calcDiff(self, data, x, u=None, recalc=True): if u is None: u = self.unone if recalc: xout, cost = self.calc(data, x, u) nq, nv = self.nq, self.nv q = a2m(x[:nq]) v = a2m(x[-nv:]) tauq = a2m(u) a = a2m(data.xout) # --- Dynamics if self.forceAba: pinocchio.computeABADerivatives(self.pinocchio, data.pinocchio, q, v, tauq) data.Fx[:, :nv] = data.pinocchio.ddq_dq data.Fx[:, nv:] = data.pinocchio.ddq_dv data.Fu[:, :] = data.pinocchio.Minv else: pinocchio.computeRNEADerivatives(self.pinocchio, data.pinocchio, q, v, a) data.Fx[:, :nv] = -np.dot(data.Minv, data.pinocchio.dtau_dq) data.Fx[:, nv:] = -np.dot(data.Minv, data.pinocchio.dtau_dv) data.Fu[:, :] = data.Minv # --- Cost pinocchio.computeJointJacobians(self.pinocchio, data.pinocchio, q) pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio) self.costs.calcDiff(data.costs, x, u, recalc=False) return data.xout, data.cost
def calcDiff(self, data, x, u=None, recalc=True): self.costsData.shareMemory(data) nq, nv = self.state.nv, self.state.nq q, v = x[:nq], x[-nv:] self.actuation.calcDiff(self.actuationData, x, u) if u is None: u = self.unone if recalc: self.calc(data, x, u) pinocchio.computeJointJacobians(self.state.pinocchio, self.pinocchioData, q) # Computing the dynamics derivatives if self.enable_force: pinocchio.computeABADerivatives(self.state.pinocchio, self.pinocchioData, q, v, u) ddq_dq = self.pinocchioData.ddq_dq ddq_dv = self.pinocchioData.ddq_dv data.Fx = np.hstack([ddq_dq, ddq_dv]) + self.pinocchioData.Minv * self.actuationData.dtau_dx data.Fu = self.pinocchioData.Minv * self.actuationData.dtau_du else: pinocchio.computeRNEADerivatives(self.state.pinocchio, self.pinocchioData, q, v, data.xout) ddq_dq = self.Minv * (self.actuationData.dtau_dx[:, :nv] - self.pinocchioData.dtau_dq) ddq_dv = self.Minv * (self.actuationData.dtau_dx[:, nv:] - self.pinocchioData.dtau_dv) data.Fx = np.hstack([ddq_dq, ddq_dv]) data.Fu = self.Minv * self.actuationData.dtau_du # Computing the cost derivatives self.costs.calcDiff(self.costsData, x, u, False)
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) computeRNEADerivatives(model, data, numpy_vector_joint_pos, numpy_vector_joint_vel, numpy_vector_joint_acc) dtorques_dq = data.dtau_dq dtorques_dqd = data.dtau_dv dtorques_dqdd = data.M
def calcDiff(self, data, x, u=None, recalc=True): if u is None: u = self.unone if recalc: xout, cost = self.calc(data, x, u) nq, nv = self.nq, self.nv q = a2m(x[:nq]) v = a2m(x[-nv:]) a = a2m(data.a) fs = data.contact.forces pinocchio.computeRNEADerivatives(self.pinocchio, data.pinocchio, q, v, a, fs) pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio) # [a;-f] = K^-1 [ tau - b, -gamma ] # [a';-f'] = -K^-1 [ K'a + b' ; J'a + gamma' ] = -K^-1 [ rnea'(q,v,a,fs) ; acc'(q,v,a) ] # Derivative of the actuation model tau = tau(q,u) # dtau_dq and dtau_dv are the rnea derivatives rnea' did_dq = data.pinocchio.dtau_dq did_dv = data.pinocchio.dtau_dv # Derivative of the contact constraint # da0_dq and da0_dv are the acceleration derivatives acc' self.contact.calcDiff(data.contact, x, recalc=False) dacc_dq = data.contact.Aq dacc_dv = data.contact.Av data.Kinv = np.linalg.inv(data.K) # We separate the Kinv into the a and f rows, and the actuation and acceleration columns da_did = -data.Kinv[:nv, :nv] df_did = data.Kinv[nv:, :nv] da_dacc = -data.Kinv[:nv, nv:] df_dacc = data.Kinv[nv:, nv:] da_dq = np.dot(da_did, did_dq) + np.dot(da_dacc, dacc_dq) da_dv = np.dot(da_did, did_dv) + np.dot(da_dacc, dacc_dv) da_dtau = data.Kinv[:nv, :nv] # Add this alias just to make the code clearer df_dtau = -data.Kinv[nv:, :nv] # Add this alias just to make the code clearer # tau is a function of x and u (typically trivial in x), whose derivatives are Ax and Au dtau_dx = data.actuation.Ax dtau_du = data.actuation.Au data.Fx[:, :nv] = da_dq data.Fx[:, nv:] = da_dv data.Fx += np.dot(da_dtau, dtau_dx) data.Fu[:, :] = np.dot(da_dtau, dtau_du) data.df_dq[:, :] = np.dot(df_did, did_dq) + np.dot(df_dacc, dacc_dq) data.df_dv[:, :] = np.dot(df_did, did_dv) + np.dot(df_dacc, dacc_dv) data.df_dx += np.dot(df_dtau, dtau_dx) data.df_du[:, :] = np.dot(df_dtau, dtau_du) self.contact.setForcesDiff(data.contact, data.df_dx, data.df_du) self.costs.calcDiff(data.costs, x, u, recalc=False) return data.xout, data.cost
def test_centroidal_derivatives(self): res = pin.computeCentroidalDynamicsDerivatives(self.model,self.data,self.q,self.v,self.a) self.assertTrue(len(res) == 4) data2 = self.model.createData() pin.computeCentroidalMomentumTimeVariation(self.model,data2,self.q,self.v,self.a) self.assertApprox(self.data.hg,data2.hg) self.assertApprox(self.data.dhg,data2.dhg) data3 = self.model.createData() pin.computeRNEADerivatives(self.model,data3,self.q,self.v,self.a) res2 = pin.getCentroidalDynamicsDerivatives(self.model,data3) for k in range(4): self.assertApprox(res[k],res2[k])
def test_generalized_gravity_derivatives(self): res = pin.computeGeneralizedGravityDerivatives(self.model, self.data, self.q) data2 = self.model.createData() ref, _, _ = pin.computeRNEADerivatives(self.model, data2, self.q, self.v * 0, self.a * 0) self.assertApprox(res, ref)
def test_static_torque_derivatives(self): res = pin.computeStaticTorqueDerivatives(self.model, self.data, self.q, self.fext) data2 = self.model.createData() ref, _, _ = pin.computeRNEADerivatives(self.model, data2, self.q, self.v * 0, self.a * 0, self.fext) self.assertApprox(res, ref)
def test_rnea_derivatives(self): res = pin.computeRNEADerivatives(self.model, self.data, self.q, self.v, self.a) self.assertTrue(len(res) == 3) data2 = self.model.createData() pin.rnea(self.model, data2, self.q, self.v, self.a) self.assertApprox(self.data.ddq, data2.ddq) # With external forces res = pin.computeRNEADerivatives(self.model, self.data, self.q, self.v, self.a, self.fext) self.assertTrue(len(res) == 3) pin.rnea(self.model, data2, self.q, self.v, self.a, self.fext) self.assertApprox(self.data.ddq, data2.ddq)
def calcDiff(self, data, x, u=None, recalc=True): q, v = x[:self.state.nq], x[-self.state.nv:] if u is None: u = self.unone if recalc: self.calc(data, x, u) pinocchio.computeJointJacobians(self.state.pinocchio, data.pinocchio, q) # Computing the dynamics derivatives if self.forceAba: pinocchio.computeABADerivatives(self.state.pinocchio, data.pinocchio, q, v, u) data.Fx = np.hstack([data.pinocchio.ddq_dq, data.pinocchio.ddq_dv]) data.Fu = data.pinocchio.Minv else: pinocchio.computeRNEADerivatives(self.state.pinocchio, data.pinocchio, q, v, data.xout) data.Fx = -np.hstack([ data.Minv * data.pinocchio.dtau_dq, data.Minv * data.pinocchio.dtau_dv ]) data.Fu = data.Minv # Computing the cost derivatives self.costs.calcDiff(data.costs, x, u, False)
## ## In this short script, we show how to compute the derivatives of the ## inverse dynamics (RNEA), using the algorithms proposed in: ## ## Analytical Derivatives of Rigid Body Dynamics Algorithms, Justin Carpentier and Nicolas Mansard, Robotics: Science and Systems, 2018 ## # Create model and data model = pin.buildSampleModelHumanoidRandom() data = model.createData() # Set bounds (by default they are undefinded for a the Simple Humanoid model) 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 the derivatives pin.computeRNEADerivatives(model, data, q, v, a) # Retrieve the derivatives in data dtau_dq = data.dtau_dq # Derivatives of the ID w.r.t. the joint config vector dtau_dv = data.dtau_dv # Derivatives of the ID w.r.t. the joint velocity vector dtau_da = data.M # Derivatives of the ID w.r.t. the joint acceleration vector
# if np.any(np.isinf(model.upperPositionLimit)): # qmin = rmodel.lowerPositionLimit # qmin[:7] = -1 # rmodel.lowerPositionLimit = qmin # qmax = rmodel.upperPositionLimit # qmax[:7] = 1 # rmodel.upperPositionLimit = qmax q = pinocchio.randomConfiguration(model) v = rand(model.nv) * 2 - 1 a = rand(model.nv) * 2 - 1 tau = pinocchio.rnea(model, data, q, v, a) # Basic check of calling the derivatives. pinocchio.computeABADerivatives(model, data, q, v, tau) pinocchio.computeRNEADerivatives(model, data, q, v, a) ''' a = M^-1 ( tau - b) a' = -M^-1 M' M^-1 (tau-b) - M^-1 b' = -M^-1 M' a - M^-1 b' = -M^-1 (M'a + b') = -M^-1 tau' ''' # Check that a = J aq + Jdot vq M = data.M Mi = pinocchio.computeMinverse(model, data, q) da = data.ddq_dq dtau = data.dtau_dq
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
def calcDiff(self, q): pin.computeABADerivatives(robot.model, robot.data, q, self.v0, self.v0) pin.computeRNEADerivatives(robot.model, robot.data, q, self.v0, self.v0) return -robot.data.dtau_dq.T @ robot.data.ddq - robot.data.ddq_dq.T @ robot.data.tau
def calcDiff(self, q): pin.computeABADerivatives(self.rmodel, self.rdata, q, self.v0, self.v0) pin.computeRNEADerivatives(self.rmodel, self.rdata, q, self.v0, self.v0) return -self.rdata.dtau_dq.T @ self.rdata.ddq - self.rdata.ddq_dq.T @ self.rdata.tau
# = -K^-1 [ M'(vnext-v) + J'^T f ] # [ J' vnext ] # Check M'(vnext-v) # Check M(vnext-v) def Mdv(q, vnext, v): M = pinocchio.crba(rmodel, rdata, q) return M * (vnext - v) dn = df_dq(rmodel, lambda _q: Mdv(_q, a2m(data.vnext), v), q) g = rmodel.gravity rmodel.gravity = pinocchio.Motion.Zero() pinocchio.computeRNEADerivatives(rmodel, rdata, q, zero(rmodel.nv), a2m(data.vnext) - v) d = rdata.dtau_dq.copy() rmodel.gravity = g assertNumDiff( d, dn, NUMDIFF_MODIFIER * mnum.disturbance ) # threshold was 1e-5, is now 2.11e-4 (see assertNumDiff.__doc__) # Check J.T f np.set_printoptions(precision=4, linewidth=200, suppress=True) def Jtf(q, f): pinocchio.computeJointJacobians(rmodel, rdata, q) pinocchio.forwardKinematics(rmodel, rdata, q) pinocchio.updateFramePlacements(rmodel, rdata) J = pinocchio.getFrameJacobian(rmodel, rdata, CONTACTFRAME,
## ## In this short script, we show how to compute the derivatives of the ## inverse dynamics (RNEA), using the algorithms proposed in: ## ## Analytical Derivatives of Rigid Body Dynamics Algorithms, Justin Carpentier and Nicolas Mansard, Robotics: Science and Systems, 2018 ## # Create model and data model = pin.buildSampleModelHumanoidRandom() data = model.createData() # Set bounds (by default they are undefinded for a the Simple Humanoid model) 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 the derivatives pin.computeRNEADerivatives(model,data,q,v,a) # Retrieve the derivatives in data dtau_dq = data.dtau_dq # Derivatives of the ID w.r.t. the joint config vector dtau_dv = data.dtau_dv # Derivatives of the ID w.r.t. the joint velocity vector dtau_da = data.M # Derivatives of the ID w.r.t. the joint acceleration vector