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(data.actuation.a) pinocchio.computeABADerivatives(self.pinocchio, data.pinocchio, q, v, tauq) da_dq = data.pinocchio.ddq_dq da_dv = data.pinocchio.ddq_dv da_dact = data.pinocchio.Minv dact_dx = data.actuation.Ax dact_du = data.actuation.Au data.Fx[:, :nv] = da_dq data.Fx[:, nv:] = da_dv data.Fx += np.dot(da_dact, dact_dx) data.Fu[:, :] = np.dot(da_dact, dact_du) 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): 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): 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 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 f(self, x, u, t, jacobian=False): nq = self.robot.nq nv = self.robot.nv model = self.robot.model data = self.robot.data q = x[:nq] v = x[nq:] if (nv == 1): # for 1 DoF systems pin.aba does not work (I don't know why) pin.computeAllTerms(model, data, q, v) ddq = (u - data.nle) / data.M[0] else: ddq = pin.aba(model, data, q, v, u) self.dx[:nv] = v self.dx[nv:] = ddq if (jacobian): pin.computeABADerivatives(model, data, q, v, u) self.Fx[:nv, :nv] = 0.0 self.Fx[:nv, nv:] = np.identity(nv) self.Fx[nv:, :nv] = data.ddq_dq self.Fx[nv:, nv:] = data.ddq_dv self.Fu[nv:, :] = data.Minv return (np.copy(self.dx), np.copy(self.Fx), np.copy(self.Fu)) return np.copy(self.dx)
def test_aba_derivatives(self): res = pin.computeABADerivatives(self.model,self.data,self.q,self.v,self.tau) self.assertTrue(len(res) == 3) data2 = self.model.createData() pin.aba(self.model,data2,self.q,self.v,self.tau) self.assertApprox(self.data.ddq,data2.ddq) # With external forces res = pin.computeABADerivatives(self.model,self.data,self.q,self.v,self.tau,self.fext) self.assertTrue(len(res) == 3) pin.aba(self.model,data2,self.q,self.v,self.tau,self.fext) self.assertApprox(self.data.ddq,data2.ddq)
def jacobian(x, u): jacx = np.zeros((12, 12)) jacu = np.zeros((12, 6)) q = x[:6] dq = x[6:] a = pinocchio.computeABADerivatives(robot.model, robot.data, q, dq, u) jacx[:6, 6:] = np.eye(6) jacx[6:, :6] = robot.data.ddq_dq jacx[6:, 6:] = robot.data.ddq_dv jacu[6:, :] = robot.data.Minv return jacx, jacu
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 ## forward dynamics, using the algorithms explained 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 tau = np.matrix(np.random.rand(model.nv, 1)) # joint acceleration # Evaluate the derivatives pin.computeABADerivatives(model, data, q, v, tau) # Retrieve the derivatives in data ddq_dq = data.ddq_dq # Derivatives of the FD w.r.t. the joint config vector ddq_dv = data.ddq_dv # Derivatives of the FD w.r.t. the joint velocity vector ddq_dtau = data.Minv # Derivatives of the FD 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
for k in range(m.njoints): fext.append(pin.Force.Zero()) # Evaluate all terms and forward dynamics pin.computeAllTerms(m, d, q, v) a = pin.aba(m, d, q, v, u, fext) # Print state and controls print("pos =", q) print("vel =", v) print("tau =", u) print("acc =", a) print("nle =", d.nle) # Evaluate the ABA derivatives pin.computeABADerivatives(m, d, q, v, u, fext) print("\nABA derivatives:") print("ddq_dq:\n", d.ddq_dq) print("ddq_dv:\n", d.ddq_dv) print("M:\n", d.M) print("Minv:\n", d.Minv) # Evaluate the sensitivities of M & Minv to configuration perturbations M = d.M Minv = d.Minv dM_dq = np.zeros((m.nv, m.nv)) eps = 1e-6 for i in range(m.nv): q[i] += eps print("pos =", q) pin.computeAllTerms(m, d, q, v)
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
def f(self, x, u, t, jacobian=False): nq = self.robot.nq nv = self.robot.nv model = self.robot.model data = self.robot.data q = x[:nq] v = x[nq:] i = 0 self.robot.computeAllTerms(q, v) for cs in self.contact_surfaces: # for each candidate contact surface for cp in self.contact_points: # for each candidate contact point # Contact point placement H = self.robot.framePlacement(q, cp.frame_id, False) p_c = H.translation if (cs.check_collision(p_c) ): # check whether the point is colliding with the surface if (cp.active == False ): # if the contact was not already active cp.active = True cp.p0 = np.copy(p_c) # anchor point # Compute the contact force self.fc[i:i + 3] = cs.compute_force(cp, q, v, self.robot) # compute the jacobian self.Jc[i:i + 3, :] = cp.get_jacobian() i += 3 else: # if the point is not colliding more if (cp.active): # if the contact was already active cp.active = False # Contact force equal to 0 self.fc[i:i + 3] = np.zeros(3) # jacobian equl to zero self.Jc[i:i + 3, :] = np.zeros((3, self.robot.model.nv)) i += 3 # compute JT*force from contact point u_con = u + self.Jc.T.dot(self.fc) if (nv == 1): # for 1 DoF systems pin.aba does not work (I don't know why) ddq = (u_con - data.nle) / data.M[0] else: ddq = pin.aba(model, data, q, v, u_con) self.dx[:nv] = v self.dx[nv:] = ddq if (jacobian): pin.computeABADerivatives(model, data, q, v, u_con) self.Fx[:nv, :nv] = 0.0 self.Fx[:nv, nv:] = np.identity(nv) self.Fx[nv:, :nv] = data.ddq_dq self.Fx[nv:, nv:] = data.ddq_dv self.Fu[nv:, :] = data.Minv return (np.copy(self.dx), np.copy(self.Fx), np.copy(self.Fu)) return np.copy(self.dx)
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
## ## In this short script, we show how to compute the derivatives of the ## forward dynamics, using the algorithms explained 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 tau = np.matrix(np.random.rand(model.nv,1)) # joint acceleration # Evaluate the derivatives pin.computeABADerivatives(model,data,q,v,tau) # Retrieve the derivatives in data ddq_dq = data.ddq_dq # Derivatives of the FD w.r.t. the joint config vector ddq_dv = data.ddq_dv # Derivatives of the FD w.r.t. the joint velocity vector ddq_dtau = data.Minv # Derivatives of the FD w.r.t. the joint acceleration vector