def __init__(self, pinocchioModel, actuationModel): self.pinocchio = pinocchioModel self.actuation = actuationModel self.State = StatePinocchio(self.pinocchio) self.costs = CostModelSum(self.pinocchio) self.nq, self.nv = self.pinocchio.nq, self.pinocchio.nv self.nx = self.State.nx self.ndx = self.State.ndx self.nout = self.nv self.nu = self.actuation.nu self.unone = np.zeros(self.nu)
def runningModel(contactIds, effectors, com=None, integrationStep=1e-2): ''' Creating the action model for floating-base systems. A walker system is by default a floating-base system. contactIds is a list of frame Ids of points that should be in contact. effectors is a dict of key frame ids and SE3 values of effector references. ''' actModel = ActuationModelFreeFloating(rmodel) State = StatePinocchio(rmodel) # Creating a 6D multi-contact model, and then including the supporting foot contactModel = ContactModelMultiple(rmodel) for cid in contactIds: contactModel.addContact('contact%d' % cid, ContactModel6D(rmodel, cid, ref=None)) # Creating the cost model for a contact phase costModel = CostModelSum(rmodel, actModel.nu) wx = np.array([0] * 6 + [.1] * (rmodel.nv - 6) + [10] * rmodel.nv) costModel.addCost('xreg', weight=1e-1, cost=CostModelState( rmodel, State, ref=rmodel.defaultState, nu=actModel.nu, activation=ActivationModelWeightedQuad(wx))) costModel.addCost('ureg', weight=1e-4, cost=CostModelControl(rmodel, nu=actModel.nu)) for fid, ref in effectors.items(): if not isinstance(ref, SE3): ref = SE3(eye(3), a2m(ref)) costModel.addCost("track%d" % fid, weight=100., cost=CostModelFramePlacement(rmodel, fid, ref, actModel.nu)) if com is not None: costModel.addCost("com", weight=100., cost=CostModelCoM(rmodel, ref=com, nu=actModel.nu)) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = DifferentialActionModelFloatingInContact(rmodel, actModel, contactModel, costModel) model = IntegratedActionModelEuler(dmodel) model.timeStep = integrationStep return model
def impactModel(contactIds, effectors): State = StatePinocchio(rmodel) # Creating a 6D multi-contact model, and then including the supporting foot impulseModel = ImpulseModelMultiple(rmodel, {"impulse%d" % cid: ImpulseModel6D(rmodel, cid) for cid in contactIds}) # Creating the cost model for a contact phase costModel = CostModelSum(rmodel, nu=0) wx = np.array([0] * 6 + [.1] * (rmodel.nv - 6) + [10] * rmodel.nv) costModel.addCost('xreg', weight=.1, cost=CostModelState(rmodel, State, ref=rmodel.defaultState, nu=0, activation=ActivationModelWeightedQuad(wx))) costModel.addCost('com', weight=1., cost=CostModelImpactCoM(rmodel, activation=ActivationModelWeightedQuad(m2a([.1, .1, 3.])))) for fid, ref in effectors.items(): costModel.addCost("track%d" % fid, weight=100., cost=CostModelFramePlacement(rmodel, fid, ref, nu=0)) # costModel.addCost("vel%d"%fid, weight=0., # cost = CostModelFrameVelocity(rmodel,fid,nu=0)) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme model = ActionModelImpact(rmodel, impulseModel, costModel) return model
def __init__(self, pinocchioModel, frameName='gripper_left_fingertip_2_link'): costModel = CostModelSum(pinocchioModel) DifferentialActionModelFullyActuated.__init__(self, pinocchioModel, costModel) self.costs.addCost(name="pos", weight=10, cost=CostModelFrameTranslation(pinocchioModel, pinocchioModel.getFrameId(frameName), np.array([.5, .4, .3]))) self.costs.addCost(name="regx", weight=0.1, cost=CostModelState(pinocchioModel, self.State, self.State.zero(), activation=act1)) self.costs.addCost(name="regu", weight=0.01, cost=CostModelControl(pinocchioModel))
def __init__(self): robot = self.robot = loadTalosArm(freeFloating=True) rmodel = self.rmodel = robot.model qmin = rmodel.lowerPositionLimit qmin[:7] = -1 rmodel.lowerPositionLimit = qmin qmax = rmodel.upperPositionLimit qmax[:7] = 1 rmodel.upperPositionLimit = qmax State = self.State = StatePinocchio(rmodel) actModel = self.actModel = ActuationModelFreeFloating(rmodel) contactModel = self.contactModel = ContactModelMultiple(rmodel) contact6 = ContactModel6D(rmodel, rmodel.getFrameId(contactName), ref=pinocchio.SE3.Identity(), gains=[0., 0.]) contactModel.addContact(name='contact', contact=contact6) costModel = self.costModel = CostModelSum(rmodel, nu=actModel.nu) self.cost1 = CostModelFrameTranslation( rmodel, nu=actModel.nu, frame=rmodel.getFrameId(opPointName), ref=np.array([.5, .4, .3])) stateWeights = np.array([0] * 6 + [0.01] * (rmodel.nv - 6) + [10] * rmodel.nv) self.cost2 = CostModelState(rmodel, State, ref=State.zero(), nu=actModel.nu, activation=ActivationModelWeightedQuad( stateWeights**2)) self.cost3 = CostModelControl(rmodel, nu=actModel.nu) costModel.addCost(name="pos", weight=10, cost=self.cost1) costModel.addCost(name="regx", weight=0.1, cost=self.cost2) costModel.addCost(name="regu", weight=0.01, cost=self.cost3) self.dmodel = DifferentialActionModelFloatingInContact( rmodel, actModel, contactModel, costModel) self.model = IntegratedActionModelEuler(self.dmodel) self.data = self.model.createData() self.cd1 = self.data.differential.costs.costs['pos'] self.cd2 = self.data.differential.costs.costs['regx'] self.cd3 = self.data.differential.costs.costs['regu'] self.ddata = self.data.differential self.rdata = self.data.differential.pinocchio self.x = self.State.rand() self.q = a2m(self.x[:rmodel.nq]) self.v = a2m(self.x[rmodel.nq:]) self.u = np.random.rand(self.model.nu)
def __init__(self): robot = self.robot = loadTalosArm() rmodel = self.rmodel = robot.model rmodel.armature = np.matrix([0] * rmodel.nv).T for j in rmodel.joints[1:]: if j.shortname() != 'JointModelFreeFlyer': rmodel.armature[j.idx_v:j.idx_v + j.nv] = 1 State = self.State = StatePinocchio(rmodel) self.cost1 = CostModelFrameTranslation( rmodel, nu=rmodel.nv, frame=rmodel.getFrameId(opPointName), ref=np.array([.5, .4, .3])) self.cost2 = CostModelState(rmodel, State, ref=State.zero(), nu=rmodel.nv) self.cost3 = CostModelControl(rmodel, nu=rmodel.nv) costModel = CostModelSum(rmodel) costModel.addCost(name="pos", weight=10, cost=self.cost1) costModel.addCost(name="regx", weight=0.1, cost=self.cost2) costModel.addCost(name="regu", weight=0.01, cost=self.cost3) self.dmodel = DifferentialActionModelFullyActuated(rmodel, costModel) self.model = IntegratedActionModelEuler(self.dmodel) self.data = self.model.createData() self.cd1 = self.data.differential.costs.costs['pos'] self.cd2 = self.data.differential.costs.costs['regx'] self.cd3 = self.data.differential.costs.costs['regu'] self.ddata = self.data.differential self.rdata = self.data.differential.pinocchio self.x = self.State.rand() self.q = a2m(self.x[:rmodel.nq]) self.v = a2m(self.x[rmodel.nq:]) self.u = np.random.rand(self.model.nu)
def createModels(self, timeStep, supportFootIds, comTask=None): # Creating the action model for floating-base systems actModel = ActuationModelFreeFloating(self.rmodel) # Creating a 3D multi-contact model, and then including the supporting # feet contactModel = ContactModelMultiple(self.rmodel) for i in supportFootIds: supportContactModel = ContactModel3D(self.rmodel, i, ref=[0., 0., 0.], gains=[0., 0.]) contactModel.addContact('contact_' + str(i), supportContactModel) # Creating the cost model for a contact phase costModel = CostModelSum(self.rmodel, actModel.nu) # CoM tracking cost if isinstance(comTask, np.ndarray): comTrack = CostModelCoM(self.rmodel, comTask, actModel.nu) costModel.addCost("comTrack", comTrack, 1e2) # State and control regularization stateWeights = np.array([0] * 6 + [0.01] * (self.rmodel.nv - 6) + [10] * self.rmodel.nv) stateReg = CostModelState(self.rmodel, self.state, self.rmodel.defaultState, actModel.nu, ActivationModelWeightedQuad(stateWeights**2)) ctrlReg = CostModelControl(self.rmodel, actModel.nu) costModel.addCost("stateReg", stateReg, 1e-1) costModel.addCost("ctrlReg", ctrlReg, 1e-4) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = DifferentialActionModelFloatingInContact( self.rmodel, actModel, contactModel, costModel) model = IntegratedActionModelEuler(dmodel) model.timeStep = timeStep return model
def impactModel(contactIds, effectors): ''' Creating the action model for floating-base systems during the impact phase. :params contactIds: list of frame Ids of points that should be in contact. :params effectors: dict of key frame ids and SE3 values of effector references. This value should typically be provided for effector landing. ''' State = StatePinocchio(rmodel) # Creating a 6D multi-contact model, and then including the supporting foot impulseModel = ImpulseModelMultiple( rmodel, {"impulse%d" % cid: ImpulseModel6D(rmodel, cid) for cid in contactIds}) # Creating the cost model for a contact phase costModel = CostModelSum(rmodel, nu=0) wx = np.array([0] * 6 + [.1] * (rmodel.nv - 6) + [10] * rmodel.nv) costModel.addCost('xreg', weight=1e-1, cost=CostModelState( rmodel, State, ref=rmodel.defaultState, nu=0, activation=ActivationModelWeightedQuad(wx))) # costModel.addCost('com',weight=1., # cost=CostModelImpactCoM(rmodel, # activation=ActivationModelWeightedQuad(m2a([.1,.1,3.])))) for fid, ref in effectors.items(): wp = np.array([1.] * 6) costModel.addCost("track%d" % fid, weight=1e5, cost=CostModelFramePlacement( rmodel, fid, ref, nu=0, activation=ActivationModelWeightedQuad(wp))) # costModel.addCost("vel%d"%fid, weight=0., # cost = CostModelFrameVelocity(rmodel,fid,nu=0)) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme model = ActionModelImpact(rmodel, impulseModel, costModel) return model
np.set_printoptions(linewidth=400, suppress=True) State = StatePinocchio(rmodel) actModel = ActuationModelFreeFloating(State) gains = pinocchio.utils.rand(2) Mref_lf = FramePlacement(rmodel.getFrameId('LF_FOOT'), pinocchio.SE3.Random()) contactModel6 = ContactModel6D(State, Mref_lf, actModel.nu, gains) rmodel.frames[Mref_lf.frame].placement = pinocchio.SE3.Random() contactModel = ContactModelMultiple(State, actModel.nu) contactModel.addContact("LF_FOOT_contact", contactModel6) contactData = contactModel.createData(rdata) model = DifferentialActionModelFloatingInContact( State, actModel, contactModel, CostModelSum(State, actModel.nu, False), 0., True) data = model.createData() model.calc(data, x, u) model.calcDiff(data, x, u) mnum = DifferentialActionModelNumDiff(model, False) dnum = mnum.createData() mnum.calcDiff(dnum, x, u) model.costs.addCost( "momentum", CostModelMomentum(State, a2m(np.random.rand(6)), actModel.nu), 1.)
assertNumDiff( dnum.Fx, data.Fx, NUMDIFF_MODIFIER * mnum.disturbance ) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff( dnum.Rx, data.Rx, NUMDIFF_MODIFIER * mnum.disturbance ) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff( dnum.Lx, data.Lx, NUMDIFF_MODIFIER * mnum.disturbance ) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__) assert (data.Fu.shape[1] == 0 and (data.Lu == 0 or data.Lu.shape == (0, ))) # ---------------------------------------------------------------------- # --- CHECK WITH SUM OF COSTS ------------------------------------------ # ---------------------------------------------------------------------- model.costs = CostModelSum(rmodel, nu=0) model.costs.addCost(cost=costModel, weight=1, name="impactwb") data = model.createData() model.calc(data, x) model.calcDiff(data, x) mnum = ActionModelNumDiff(model, withGaussApprox=True) dnum = mnum.createData() nx, ndx, nq, nv, nu = model.nx, model.ndx, model.nq, model.nv, model.nu mnum.calcDiff(dnum, x, None) assertNumDiff( dnum.Fx[:nv, :nv], data.Fx[:nv, :nv], NUMDIFF_MODIFIER * mnum.disturbance ) # threshold was 1e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
def createMultiphaseShootingProblem(rmodel, rdata, csw, timeStep): """ Create a Multiphase Shooting problem from the output of the centroidal solver. :params rmodel: robot model of type pinocchio::model :params rdata: robot data of type pinocchio::data :params csw: contact sequence wrapper of type ContactSequenceWrapper :params timeStep: Scalar timestep between nodes. :returns list of IntegratedActionModels """ # ----------------------- # Define Cost weights class Weights: com = 1e1 regx = 1e-3 regu = 0. swing_patch = 1e6 forces = 0. contactv = 1e3 # Define state cost vector for WeightedActivation ff_orientation = 1e1 xweight = np.array([0] * 3 + [ff_orientation] * 3 + [1.] * (rmodel.nv - 6) + [1.] * rmodel.nv) xweight[range(18, 20)] = ff_orientation # for patch in swing_patch: w.swing_patch.append(100.); # Define weights for the impact costs. imp_state = 1e2 imp_com = 1e2 imp_contact_patch = 1e6 imp_act_com = m2a([0.1, 0.1, 3.0]) # Define weights for the terminal costs term_com = 1e8 term_regx = 1e4 w = Weights() # ------------------------ problem_models = [] actuationff = ActuationModelFreeFloating(rmodel) State = StatePinocchio(rmodel) active_contact_patch = set() active_contact_patch_prev = set() for nphase, phase in enumerate(csw.cs.contact_phases): t0 = phase.time_trajectory[0] t1 = phase.time_trajectory[-1] N = int(round((t1 - t0) / timeStep)) + 1 contact_model = ContactModelMultiple(rmodel) # Add contact constraints for the active contact patches. # Add SE3 cost for the non-active contact patches. swing_patch = [] active_contact_patch_prev = active_contact_patch.copy() active_contact_patch.clear() for patch in csw.ee_map.keys(): if getattr(phase, patch).active: active_contact_patch.add(patch) active_contact = ContactModel6D(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch]), ref=getattr(phase, patch).placement) contact_model.addContact(patch, active_contact) # print nphase, "Contact ",patch," added at ", getattr(phase,patch).placement.translation.T else: swing_patch.append(patch) # Check if contact has been added in this phase. If this phase is not zero, # add an impulse model to deal with this contact. new_contacts = active_contact_patch.difference(active_contact_patch_prev) if nphase != 0 and len(new_contacts) != 0: # print nphase, "Impact ",[p for p in new_contacts]," added" imp_model = ImpulseModelMultiple( rmodel, { "Impulse_" + patch: ImpulseModel6D(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch])) for patch in new_contacts }) # Costs for the impulse of a new contact cost_model = CostModelSum(rmodel, nu=0) # State cost_regx = CostModelState(rmodel, State, ref=rmodel.defaultState, nu=actuationff.nu, activation=ActivationModelWeightedQuad(w.xweight)) cost_model.addCost("imp_regx", cost_regx, w.imp_state) # CoM cost_com = CostModelImpactCoM(rmodel, activation=ActivationModelWeightedQuad(w.imp_act_com)) cost_model.addCost("imp_CoM", cost_com, w.imp_com) # Contact Frameplacement for patch in new_contacts: cost_contact = CostModelFramePlacement(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch]), ref=SE3(np.identity(3), csw.ee_splines[patch].eval(t0)[0]), nu=actuationff.nu) cost_model.addCost("imp_contact_" + patch, cost_contact, w.imp_contact_patch) imp_action_model = ActionModelImpact(rmodel, imp_model, cost_model) problem_models.append(imp_action_model) # Define the cost and action models for each timestep in the contact phase. # untill [:-1] because in contact sequence timetrajectory, the end-time is # also included. e.g., instead of being [0.,0.5], time trajectory is [0,0.5,1.] for t in np.linspace(t0, t1, N)[:-1]: cost_model = CostModelSum(rmodel, actuationff.nu) # For the first node of the phase, add cost v=0 for the contacting foot. if t == 0: for patch in active_contact_patch: cost_vcontact = CostModelFrameVelocity(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch]), ref=m2a(Motion.Zero().vector), nu=actuationff.nu) cost_model.addCost("contactv_" + patch, cost_vcontact, w.contactv) # CoM Cost cost_com = CostModelCoM(rmodel, ref=m2a(csw.phi_c.com_vcom.eval(t)[0][:3, :]), nu=actuationff.nu) cost_model.addCost("CoM", cost_com, w.com) # Forces Cost for patch in contact_model.contacts.keys(): cost_force = CostModelForce(rmodel, contactModel=contact_model.contacts[patch], ref=m2a(csw.phi_c.forces[patch].eval(t)[0]), nu=actuationff.nu) cost_model.addCost("forces_" + patch, cost_force, w.forces) # Swing patch cost for patch in swing_patch: cost_swing = CostModelFramePlacement(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch]), ref=SE3(np.identity(3), csw.ee_splines[patch].eval(t)[0]), nu=actuationff.nu) cost_model.addCost("swing_" + patch, cost_swing, w.swing_patch) # print t, "Swing cost ",patch," added at ", csw.ee_splines[patch].eval(t)[0][:3].T # State Regularization cost_regx = CostModelState(rmodel, State, ref=rmodel.defaultState, nu=actuationff.nu, activation=ActivationModelWeightedQuad(w.xweight)) cost_model.addCost("regx", cost_regx, w.regx) # Control Regularization cost_regu = CostModelControl(rmodel, nu=actuationff.nu) cost_model.addCost("regu", cost_regu, w.regu) dmodel = DifferentialActionModelFloatingInContact(rmodel, actuationff, contact_model, cost_model) imodel = IntegratedActionModelEuler(dmodel, timeStep=timeStep) problem_models.append(imodel) # Create Terminal Model. contact_model = ContactModelMultiple(rmodel) # Add contact constraints for the active contact patches. swing_patch = [] t = t1 for patch in csw.ee_map.keys(): if getattr(phase, patch).active: active_contact = ContactModel6D(rmodel, frame=rmodel.getFrameId(csw.ee_map[patch]), ref=getattr(phase, patch).placement) contact_model.addContact(patch, active_contact) cost_model = CostModelSum(rmodel, actuationff.nu) # CoM Cost cost_com = CostModelCoM(rmodel, ref=m2a(csw.phi_c.com_vcom.eval(t)[0][:3, :]), nu=actuationff.nu) cost_model.addCost("CoM", cost_com, w.term_com) # State Regularization cost_regx = CostModelState(rmodel, State, ref=rmodel.defaultState, nu=actuationff.nu, activation=ActivationModelWeightedQuad(w.xweight)) cost_model.addCost("regx", cost_regx, w.term_regx) dmodel = DifferentialActionModelFloatingInContact(rmodel, actuationff, contact_model, cost_model) imodel = IntegratedActionModelEuler(dmodel) problem_models.append(imodel) problem_models.append return problem_models
class DifferentialActionModelActuated: '''Unperfect class written to validate the actuation model. Do not use except for tests. ''' def __init__(self, pinocchioModel, actuationModel): self.pinocchio = pinocchioModel self.actuation = actuationModel self.State = StatePinocchio(self.pinocchio) self.costs = CostModelSum(self.pinocchio) self.nq, self.nv = self.pinocchio.nq, self.pinocchio.nv self.nx = self.State.nx self.ndx = self.State.ndx self.nout = self.nv self.nu = self.actuation.nu self.unone = np.zeros(self.nu) @property def ncost(self): return self.costs.ncost def createData(self): return DifferentialActionDataActuated(self) def calc(self, data, x, u=None): if u is None: u = self.unone nq, nv = self.nq, self.nv q = a2m(x[:nq]) v = a2m(x[-nv:]) tauq = a2m(self.actuation.calc(data.actuation, x, u)) data.xout[:] = pinocchio.aba(self.pinocchio, data.pinocchio, q, v, tauq).flat pinocchio.forwardKinematics(self.pinocchio, data.pinocchio, q, v) pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio) data.cost = self.costs.calc(data.costs, x, u) return data.xout, data.cost 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
assertNumDiff(costData.Luu, costDataND.Luu, NUMDIFF_MODIFIER * costModelND.disturbance) # threshold was 1e-4, is now 2.11e-4 (see assertNumDiff.__doc__) X = StatePinocchio(rmodel) q = pinocchio.randomConfiguration(rmodel) v = rand(rmodel.nv) x = m2a(np.concatenate([q, v])) u = m2a(rand(rmodel.nv)) act1 = ActivationModelWeightedQuad(weights=np.array([ 1., ] * x.size)) cost1 = CostModelFrameTranslation(rmodel, rmodel.getFrameId('gripper_left_fingertip_2_link'), np.array([.5, .4, .3])) cost2 = CostModelState(rmodel, X, X.rand(), activation=act1) cost3 = CostModelControl(rmodel) costModel = CostModelSum(rmodel) costModel.addCost("pos", cost1, 10) costModel.addCost("regx", cost2, .1) costModel.addCost("regu", cost3, .01) costData = costModel.createData(rdata) pinocchio.forwardKinematics(rmodel, rdata, q, v) pinocchio.computeJointJacobians(rmodel, rdata, q) pinocchio.updateFramePlacements(rmodel, rdata) costModel.calcDiff(costData, x, u) costModelND = CostModelNumDiff( costModel, StatePinocchio(rmodel), withGaussApprox=True, reevals=[
x = np.concatenate([m2a(q), m2a(v)]) u = np.random.rand(rmodel.nv - 6) * 2 - 1 actModel = ActuationModelFreeFloating(rmodel) contactModel3 = ContactModel3D( rmodel, rmodel.getFrameId('gripper_left_fingertip_2_link'), ref=np.random.rand(3), gains=[4., 4.]) rmodel.frames[contactModel3.frame].placement = pinocchio.SE3.Random() contactModel = ContactModelMultiple(rmodel) contactModel.addContact(name='fingertip', contact=contactModel3) model = DifferentialActionModelFloatingInContact(rmodel, actModel, contactModel, CostModelSum(rmodel)) data = model.createData() model.calc(data, x, u) assert (len(list(filter(lambda x: x > 0, eig(data.K)[0]))) == model.nv) assert (len(list(filter(lambda x: x < 0, eig(data.K)[0]))) == model.ncontact) _taucheck = pinocchio.rnea(rmodel, rdata, q, v, a2m(data.a), data.contact.forces) _taucheck.flat[:] += rmodel.armature.flat * data.a assert (absmax(_taucheck[:6]) < 1e-6) assert (absmax(m2a(_taucheck[6:]) - u) < 1e-6) model.calcDiff(data, x, u) mnum = DifferentialActionModelNumDiff(model, withGaussApprox=False) dnum = mnum.createData()
np.set_printoptions(linewidth=400, suppress=True) State = StatePinocchio(rmodel) actModel = ActuationModelFreeFloating(State) gains = pinocchio.utils.rand(2) Mref_lf = FramePlacement(rmodel.getFrameId('LF_FOOT'), pinocchio.SE3.Random()) contactModel6 = ContactModel6D(State, Mref_lf, actModel.nu, gains) rmodel.frames[Mref_lf.frame].placement = pinocchio.SE3.Random() contactModel = ContactModelMultiple(State, actModel.nu) contactModel.addContact("LF_FOOT_contact", contactModel6) contactData = contactModel.createData(rdata) model = DifferentialActionModelFloatingInContact(State, actModel, contactModel, CostModelSum(State, actModel.nu), 0., True) data = model.createData() model.calc(data, x, u) model.calcDiff(data, x, u) mnum = DifferentialActionModelNumDiff(model, False) dnum = mnum.createData() mnum.calc(dnum, x, u) mnum.calcDiff(dnum, x, u) model.costs.addCost("momentum", CostModelMomentum(State, a2m(np.random.rand(6)), actModel.nu), 1.) data = model.createData()
np.set_printoptions(linewidth=400, suppress=True) State = StatePinocchio(rmodel) actModel = ActuationModelFreeFloating(State) gains = pinocchio.utils.rand(2) Mref_lf = FramePlacement(rmodel.getFrameId('LF_FOOT'), pinocchio.SE3.Random()) contactModel6 = ContactModel6D(State, Mref_lf, actModel.nu, gains) rmodel.frames[Mref_lf.frame].placement = pinocchio.SE3.Random() contactModel = ContactModelMultiple(State, actModel.nu) contactModel.addContact("LF_FOOT_contact", contactModel6) contactData = contactModel.createData(rdata) model = DifferentialActionModelFloatingInContact( State, actModel, contactModel, CostModelSum(State, actModel.nu), 0., True) data = model.createData() model.calc(data, x, u) model.calcDiff(data, x, u) mnum = DifferentialActionModelNumDiff(model, False) dnum = mnum.createData() mnum.calc(dnum, x, u) mnum.calcDiff(dnum, x, u) model.costs.addCost( "momentum", CostModelMomentum(State, a2m(np.random.rand(6)), actModel.nu), 1.)