def createProblem(self, x0, comGoTo, timeStep, numKnots): # Compute the current foot positions q0 = a2m(x0[:self.rmodel.nq]) pinocchio.forwardKinematics(self.rmodel, self.rdata, q0) pinocchio.updateFramePlacements(self.rmodel, self.rdata) com0 = m2a(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)) # Defining the action models along the time instances comModels = [] # Creating the action model for the CoM task comForwardModels = [ self.createModels( timeStep, [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId], ) for k in range(numKnots) ] comForwardTermModel = self.createModels( timeStep, [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId], com0 + [comGoTo, 0., 0.]) comForwardTermModel.differential.costs['comTrack'].weight = 1e6 # Adding the CoM tasks comModels += comForwardModels + [comForwardTermModel] # Defining the shooting problem problem = ShootingProblem(x0, comModels, comModels[-1]) return problem
def createEESplines(self, rmodel, rdata, xs, t_sampling=0.005): N = len(xs) abscissa = a2m(np.linspace(0., t_sampling * (N - 1), N)) self.ee_splines = EESplines() for patch in self.ee_map.keys(): p = np.zeros((3, N)) m = np.zeros((3, N)) for i in range(N): q = a2m(xs[i][:rmodel.nq]) v = a2m(xs[i][-rmodel.nv:]) pinocchio.forwardKinematics(rmodel, rdata, q, v) p[:, i] = m2a( pinocchio.updateFramePlacement(rmodel, rdata, rmodel.getFrameId(self.ee_map[patch])).translation) m[:, i] = m2a(pinocchio.getFrameVelocity(rmodel, rdata, rmodel.getFrameId(self.ee_map[patch])).linear) self.ee_splines.update([[patch, CubicHermiteSpline(abscissa, p, m)]]) return
def __init__(self, rmodel, lfFoot, rfFoot, lhFoot, rhFoot): self.rmodel = rmodel self.rdata = rmodel.createData() self.state = StatePinocchio(self.rmodel) # Getting the frame id for all the legs self.lfFootId = self.rmodel.getFrameId(lfFoot) self.rfFootId = self.rmodel.getFrameId(rfFoot) self.lhFootId = self.rmodel.getFrameId(lhFoot) self.rhFootId = self.rmodel.getFrameId(rhFoot) # Defining default state self.rmodel.defaultState = np.concatenate([ m2a(self.rmodel.referenceConfigurations["half_sitting"]), np.zeros(self.rmodel.nv) ])
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
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
robot.initDisplay(loadModel=True) rmodel = robot.model rdata = rmodel.createData() # Setting up the 3d walking problem rightFoot = 'right_sole_link' leftFoot = 'left_sole_link' rightId = rmodel.getFrameId(rightFoot) leftId = rmodel.getFrameId(leftFoot) # Create the initial state q0 = rmodel.referenceConfigurations['half_sitting'].copy() v0 = zero(rmodel.nv) x0 = m2a(np.concatenate([q0, v0])) rmodel.defaultState = x0.copy() # Solving the 3d walking problem using DDP stepLength = 0.2 swingDuration = 0.75 stanceDurantion = 0.1 def dodisp(xs, dt): return displayTrajectory(robot, xs, dt) disp = dodisp if WITHDISPLAY else lambda xs, dt: 0 disp.__defaults__ = (.1, )
dmodel = DifferentialActionModelFloatingInContact( self.rmodel, actModel, contactModel, costModel) model = IntegratedActionModelEuler(dmodel) model.timeStep = timeStep return model # Loading the HyQ model robot = loadHyQ() rmodel = robot.model rdata = rmodel.createData() # Defining the initial state of the robot q0 = rmodel.referenceConfigurations['half_sitting'].copy() v0 = pinocchio.utils.zero(rmodel.nv) x0 = m2a(np.concatenate([q0, v0])) # Setting up the 3d walking problem lfFoot = 'lf_foot' rfFoot = 'rf_foot' lhFoot = 'lh_foot' rhFoot = 'rh_foot' walk = SimpleQuadrupedProblem(rmodel, lfFoot, rfFoot, lhFoot, rhFoot) # Setting up the walking variables comGoTo = 0.1 # meters timeStep = 5e-2 # seconds supportKnots = 2 # Creating the CoM problem and solving it ddp = SolverDDP(walk.createProblem(x0, comGoTo, timeStep, supportKnots))
# ----------------------------------------------------------------------------- robot = loadTalosArm(freeFloating=True) qmin = robot.model.lowerPositionLimit qmin[:7] = -1 robot.model.lowerPositionLimit = qmin qmax = robot.model.upperPositionLimit qmax[:7] = 1 robot.model.upperPositionLimit = qmax rmodel = robot.model rdata = rmodel.createData() actModel = ActuationModelFreeFloating(rmodel) model = DifferentialActionModelActuated(rmodel, actModel) data = model.createData() q = pinocchio.randomConfiguration(rmodel) v = rand(rmodel.nv) x = m2a(np.concatenate([q, v])) u = m2a(rand(rmodel.nv - 6)) model.calcDiff(data, x, u) mnum = DifferentialActionModelNumDiff(model) dnum = mnum.createData() mnum.calcDiff(dnum, x, u) assertNumDiff(data.Fx, dnum.Fx, NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 2.7e-2, is now 2.11e-4 (see assertNumDiff.__doc__) assertNumDiff(data.Fu, dnum.Fu, NUMDIFF_MODIFIER * mnum.disturbance) # threshold was 7e-3, is now 2.11e-4 (see assertNumDiff.__doc__)
rmodel = robot.model rdata = rmodel.createData() rmodel.q0 = rmodel.referenceConfigurations['half_sitting'].copy() # Setting up the 3d walking problem rightFoot = 'right_sole_link' leftFoot = 'left_sole_link' rightId = rmodel.getFrameId(rightFoot) leftId = rmodel.getFrameId(leftFoot) # Create the initial state q0 = rmodel.q0 v0 = zero(rmodel.nv) x0 = m2a(np.concatenate([q0, v0])) rmodel.defaultState = x0.copy() 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)
from pinocchio.utils import rand, zero from testutils import NUMDIFF_MODIFIER, assertNumDiff robot = loadTalosArm() qmin = robot.model.lowerPositionLimit qmin[:7] = -1 robot.model.lowerPositionLimit = qmin qmax = robot.model.upperPositionLimit qmax[:7] = 1 robot.model.upperPositionLimit = qmax rmodel = robot.model rdata = rmodel.createData() q = pinocchio.randomConfiguration(rmodel) v = rand(rmodel.nv) x = m2a(np.concatenate([q, v])) u = m2a(rand(rmodel.nv)) costModel = CostModelFrameTranslation(rmodel, rmodel.getFrameId('gripper_left_fingertip_2_link'), np.array([.5, .4, .3])) 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,
def createCentroidalPhi(self, rmodel, rdata): # centroidal planar (muscod) returns the forces in the sequence RF,LF,RH,LH. # TODO: make more generic range_def = OrderedDict() range_def.update([["RF_patch", range(0, 6)]]) range_def.update([["LF_patch", range(6, 12)]]) range_def.update([["RH_patch", range(12, 18)]]) range_def.update([["LH_patch", range(18, 24)]]) patch_names = self.ee_map.keys() mass = pinocchio.crba(rmodel, rdata, pinocchio.neutral(rmodel))[0, 0] t_traj = None # -----Get Length of Timeline------------------------ t_traj = [] for spl in self.cs.ms_interval_data[:-1]: t_traj += list(spl.time_trajectory) t_traj = np.array(t_traj) N = len(t_traj) # ------Get values of state and control-------------- class PhiC: f = OrderedDict() df = OrderedDict() for patch in patch_names: f.update([[patch, np.zeros((N, 6))]]) df.update([[patch, np.zeros((N, 6))]]) com_vcom = np.zeros((N, 6)) vcom_acom = np.zeros((N, 6)) hg = np.zeros((N, 6)) dhg = np.zeros((N, 6)) phi_c_ = PhiC() n = 0 for i, spl in enumerate(self.cs.ms_interval_data[:-1]): x = m2a(spl.state_trajectory) dx = m2a(spl.dot_state_trajectory) u = m2a(spl.control_trajectory) nt = len(x) tt = t_traj[n:n + nt] phi_c_.com_vcom[n:n + nt, :] = x[:, :6] phi_c_.vcom_acom[n:n + nt, :] = dx[:, :6] phi_c_.hg[n:n + nt, 3:] = x[:, -3:] phi_c_.dhg[n:n + nt, 3:] = dx[:, -3:] phi_c_.hg[n:n + nt, :3] = mass * x[:, 3:6] phi_c_.dhg[n:n + nt, :3] = mass * dx[:, 3:6] # --Control output of MUSCOD is a discretized piecewise polynomial. # ------Convert the one piece to Points and Derivatives. poly_u, dpoly_u = polyfitND(tt, u, deg=3, full=True, eps=1e-5) def f_poly(t, r): return np.array([poly_u[i](t) for i in r]) def f_dpoly(t, r): return np.array([dpoly_u[i](t) for i in r]) for patch in patch_names: phi_c_.f[patch][n:n + nt, :] = np.array([f_poly(t, range_def[patch]) for t in tt]) phi_c_.df[patch][n:n + nt, :] = np.array([f_dpoly(t, range_def[patch]) for t in tt]) n += nt duplicates = findDuplicates(t_traj) class PhiC2: f = OrderedDict() df = OrderedDict() for patch in patch_names: f.update([[patch, removeDuplicates(phi_c_.f[patch], duplicates)]]) df.update([[patch, removeDuplicates(phi_c_.df[patch], duplicates)]]) com_vcom = removeDuplicates(phi_c_.com_vcom, duplicates) vcom_acom = removeDuplicates(phi_c_.vcom_acom, duplicates) hg = removeDuplicates(phi_c_.hg, duplicates) dhg = removeDuplicates(phi_c_.dhg, duplicates) phi_c_2 = PhiC2() t_traj = removeDuplicates(t_traj, duplicates) self.phi_c.com_vcom = CubicHermiteSpline(a2m(t_traj), a2m(phi_c_2.com_vcom), a2m(phi_c_2.vcom_acom)) self.phi_c.hg = CubicHermiteSpline(a2m(t_traj), a2m(phi_c_2.hg), a2m(phi_c_2.dhg)) for patch in patch_names: self.phi_c.forces[patch] = CubicHermiteSpline(a2m(t_traj), a2m(phi_c_2.f[patch]), a2m(phi_c_2.df[patch])) return
import conf_talos_warm_start as conf import numpy as np import pinocchio from crocoddyl import (ActionModelImpact, CallbackDDPVerbose, CallbackSolverDisplay, ShootingProblem, SolverDDP, StatePinocchio, a2m, m2a) from crocoddyl.locomotion import ContactSequenceWrapper, createMultiphaseShootingProblem from multicontact_api import ContactSequenceHumanoid, CubicHermiteSpline np.set_printoptions(linewidth=400, suppress=True) robot = conf.robot rmodel = robot.model rdata = robot.data rmodel.defaultState = np.concatenate([m2a(robot.q0), np.zeros(rmodel.nv)]) # ----------------Load Contact Phases and PostProcess----------------------- cs = ContactSequenceHumanoid(0) cs.loadFromXML(conf.MUSCOD_CS_OUTPUT_FILENAME, conf.CONTACT_SEQUENCE_XML_TAG) csw = ContactSequenceWrapper(cs, conf.contact_patches) csw.createCentroidalPhi(rmodel, rdata) csw.createEESplines(rmodel, rdata, conf.X_init, 0.005) # ----------------Define Problem---------------------------- models = createMultiphaseShootingProblem(rmodel, rdata, csw, conf.DT) # disp = lambda xs: disptraj(robot, xs) problem = ShootingProblem(m2a(conf.X_init[0]), models[:-1], models[-1]) # Set contacts in the data elements. Ugly. # This is defined for IAMEuler. If using IAMRK4, differential is a list. so we need to change.
def get_y(q, v): x_ = np.vstack([q, v]) model.calc(data, m2a(x_), u) return [a2m(y) for y in data.y]
def get_au(u): a, _ = model.differential.calc(data.differential[0], x, m2a(u)) return a2m(a)
def get_xn(u): model.calc(data, x, m2a(u)) return a2m(data.xnext) # .copy()
def get_ku(u): model.calc(data, x, m2a(u)) return [a2m(ki) for ki in data.ki]
def get_k(q, v): x_ = np.vstack([q, v]) model.calc(data, m2a(x_), u) return [a2m(ki) for ki in data.ki]
def get_attr_analytical(x, u, attr): _u = m2a(u) _x = m2a(x) model.calcDiff(data, _x, _u) return a2m(getattr(data, attr)) # .copy()
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
rmodel = robot.model rdata = rmodel.createData() np.set_printoptions(linewidth=400, suppress=True) 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)
def x_eval(t): return m2a(x_spl.eval(t)[0])
def calcForces(q_, v_, u_): model.calc(data, np.concatenate([m2a(q_), m2a(v_)]), m2a(u_)) return a2m(data.f)