def createPseudoImpulseModel(self, supportFootIds, swingFootTask): """ Action model for pseudo-impulse models. A pseudo-impulse model consists of adding high-penalty cost for the contact velocities. :param swingFootTask: swinging foot task :return pseudo-impulse differential action model """ # Creating a 6D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: Mref = crocoddyl.FramePlacement(i, pinocchio.SE3.Identity()) supportContactModel = crocoddyl.ContactModel6D( self.state, Mref, self.actuation.nu, np.array([0., 0.])) contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu) for i in supportFootIds: cone = crocoddyl.WrenchCone(np.identity(3), self.mu, np.array([0.1, 0.05])) wrenchCone = crocoddyl.CostModelContactWrenchCone( self.state, crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(cone.lb, cone.ub)), crocoddyl.FrameWrenchCone(i, cone), self.actuation.nu) costModel.addCost(self.rmodel.frames[i].name + "_wrenchCone", wrenchCone, 1e1) if swingFootTask is not None: for i in swingFootTask: footTrack = crocoddyl.CostModelFramePlacement( self.state, i, self.actuation.nu) costModel.addCost(self.rmodel.frames[i.id].name + "_footTrack", footTrack, 1e8) footVel = crocoddyl.FrameMotion(i.id, pinocchio.Motion.Zero()) impulseFootVelCost = crocoddyl.CostModelFrameVelocity( self.state, footVel, self.actuation.nu) costModel.addCost( self.rmodel.frames[i.id].name + "_impulseVel", impulseFootVelCost, 1e6) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.nv) stateReg = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) costModel.addCost("stateReg", stateReg, 1e1) costModel.addCost("ctrlReg", ctrlReg, 1e-3) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics( self.state, self.actuation, contactModel, costModel, 0., True) model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.) return model
def createPseudoImpulseModel(self, supportFootIds, swingFootTask): """ Action model for pseudo-impulse models. A pseudo-impulse model consists of adding high-penalty cost for the contact velocities. :param supportFootIds: Ids of the constrained feet :param swingFootTask: swinging foot task :return pseudo-impulse differential action model """ # Creating a 3D multi-contact model, and then including the supporting # foot nu = self.actuation.nu contactModel = crocoddyl.ContactModelMultiple(self.state, nu) for i in supportFootIds: supportContactModel = crocoddyl.ContactModel3D(self.state, i, np.array([0., 0., 0.]), nu, np.array([0., 50.])) contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, nu) for i in supportFootIds: cone = crocoddyl.FrictionCone(self.Rsurf, self.mu, 4, False) coneResidual = crocoddyl.ResidualModelContactFrictionCone(self.state, i, cone, nu) coneActivation = crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(cone.lb, cone.ub)) frictionCone = crocoddyl.CostModelResidual(self.state, coneActivation, coneResidual) costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1) if swingFootTask is not None: for i in swingFootTask: frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(self.state, i[0], i[1].translation, nu) frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(self.state, i[0], pinocchio.Motion.Zero(), pinocchio.LOCAL, nu) footTrack = crocoddyl.CostModelResidual(self.state, frameTranslationResidual) impulseFootVelCost = crocoddyl.CostModelResidual(self.state, frameVelocityResidual) costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack", footTrack, 1e7) costModel.addCost(self.rmodel.frames[i[0]].name + "_impulseVel", impulseFootVelCost, 1e6) stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv) stateResidual = crocoddyl.ResidualModelState(self.state, self.rmodel.defaultState, nu) stateActivation = crocoddyl.ActivationModelWeightedQuad(stateWeights**2) ctrlResidual = crocoddyl.ResidualModelControl(self.state, nu) stateReg = crocoddyl.CostModelResidual(self.state, stateActivation, stateResidual) ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual) costModel.addCost("stateReg", stateReg, 1e1) costModel.addCost("ctrlReg", ctrlReg, 1e-3) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation, contactModel, costModel, 0., True) if self.integrator == 'euler': model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.) elif self.integrator == 'rk4': model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.four, 0.) elif self.integrator == 'rk3': model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.three, 0.) elif self.integrator == 'rk2': model = crocoddyl.IntegratedActionModelRK(dmodel, crocoddyl.RKType.two, 0.) else: model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.) return model
def createPseudoImpulseModel(self, supportFootIds, swingFootTask): """ Action model for pseudo-impulse models. A pseudo-impulse model consists of adding high-penalty cost for the contact velocities. :param supportFootIds: Ids of the constrained feet :param swingFootTask: swinging foot task :return pseudo-impulse differential action model """ # Creating a 3D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: xref = crocoddyl.FrameTranslation(i, np.array([0., 0., 0.])) supportContactModel = crocoddyl.ContactModel3D( self.state, xref, self.actuation.nu, np.array([0., 50.])) contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu) for i in supportFootIds: cone = crocoddyl.FrictionCone(self.nsurf, self.mu, 4, False) frictionCone = crocoddyl.CostModelContactFrictionCone( self.state, crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(cone.lb, cone.ub)), crocoddyl.FrameFrictionCone(i, cone), self.actuation.nu) costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1) if swingFootTask is not None: for i in swingFootTask: xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation) vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero()) footTrack = crocoddyl.CostModelFrameTranslation( self.state, xref, self.actuation.nu) impulseFootVelCost = crocoddyl.CostModelFrameVelocity( self.state, vref, self.actuation.nu) costModel.addCost( self.rmodel.frames[i.frame].name + "_footTrack", footTrack, 1e7) costModel.addCost( self.rmodel.frames[i.frame].name + "_impulseVel", impulseFootVelCost, 1e6) stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * self.rmodel.nv) stateReg = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelWeightedQuad(stateWeights**2), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) costModel.addCost("stateReg", stateReg, 1e1) costModel.addCost("ctrlReg", ctrlReg, 1e-3) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics( self.state, self.actuation, contactModel, costModel, 0., True) model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.) return model
def createSwingFootModel(self, timeStep, supportFootIds, comTask=None, swingFootTask=None): """ Action model for a swing foot phase. :param timeStep: step duration of the action model :param supportFootIds: Ids of the constrained feet :param comTask: CoM task :param swingFootTask: swinging foot task :return action model for a swing foot phase """ # Creating a 3D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T) supportContactModel = crocoddyl.ContactModel3D(self.state, xref, self.actuation.nu, np.matrix([0., 50.]).T) contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu) if isinstance(comTask, np.ndarray): comTrack = crocoddyl.CostModelCoMPosition(self.state, comTask, self.actuation.nu) costModel.addCost("comTrack", comTrack, 1e6) for i in supportFootIds: cone = crocoddyl.FrictionCone(self.nsurf, self.mu, 4, False) frictionCone = crocoddyl.CostModelContactFrictionCone( self.state, crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(cone.lb, cone.ub)), cone, i, self.actuation.nu) costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1) if swingFootTask is not None: for i in swingFootTask: xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation) footTrack = crocoddyl.CostModelFrameTranslation(self.state, xref, self.actuation.nu) costModel.addCost(self.rmodel.frames[i.frame].name + "_footTrack", footTrack, 1e6) stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * 6 + [1.] * (self.rmodel.nv - 6)) stateReg = crocoddyl.CostModelState(self.state, crocoddyl.ActivationModelWeightedQuad(np.matrix(stateWeights**2).T), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) costModel.addCost("stateReg", stateReg, 1e1) costModel.addCost("ctrlReg", ctrlReg, 1e-1) lb = self.state.diff(0 * self.state.lb, self.state.lb) ub = self.state.diff(0 * self.state.ub, self.state.ub) stateBounds = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(lb, ub)), 0 * self.rmodel.defaultState, self.actuation.nu) costModel.addCost("stateBounds", stateBounds, 1e3) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation, contactModel, costModel, 0., True) model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep) return model
def createSwingFootModel(self, timeStep, supportFootIds, comTask=None, swingFootTask=None): """ Action model for a swing foot phase. :param timeStep: step duration of the action model :param supportFootIds: Ids of the constrained feet :param comTask: CoM task :param swingFootTask: swinging foot task :return action model for a swing foot phase """ # Creating a 6D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: Mref = crocoddyl.FramePlacement(i, pinocchio.SE3.Identity()) supportContactModel = \ crocoddyl.ContactModel6D(self.state, Mref, self.actuation.nu, np.matrix([0., 0.]).T) contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu) if isinstance(comTask, np.ndarray): comTrack = crocoddyl.CostModelCoMPosition(self.state, comTask, self.actuation.nu) costModel.addCost("comTrack", comTrack, 1e6) if swingFootTask is not None: for i in swingFootTask: footTrack = crocoddyl.CostModelFramePlacement( self.state, i, self.actuation.nu) costModel.addCost( self.rmodel.frames[i.frame].name + "_footTrack", footTrack, 1e6) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.nv) stateReg = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelWeightedQuad( np.matrix(stateWeights**2).T), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) costModel.addCost("stateReg", stateReg, 1e1) costModel.addCost("ctrlReg", ctrlReg, 1e-1) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics( self.state, self.actuation, contactModel, costModel) model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep) return model
def createFootSwitchModel(self, supportFootIds, swingFootTask): """ Action model for a foot switch phase. :param supportFootIds: Ids of the constrained feet :param swingFootTask: swinging foot task :return action model for a foot switch phase """ # Creating a 3D multi-contact model, and then including the supporting # foot contactModel = crocoddyl.ContactModelMultiple(self.state, self.actuation.nu) for i in supportFootIds: xref = crocoddyl.FrameTranslation(i, np.matrix([0., 0., 0.]).T) supportContactModel = crocoddyl.ContactModel3D( self.state, xref, self.actuation.nu, np.matrix([0., 0.]).T) contactModel.addContact('contact_' + str(i), supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, self.actuation.nu) if swingFootTask is not None: for i in swingFootTask: xref = crocoddyl.FrameTranslation(i.frame, i.oMf.translation) footTrack = crocoddyl.CostModelFrameTranslation( self.state, xref, self.actuation.nu) costModel.addCost("footTrack_" + str(i), footTrack, 1e7) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10] * self.rmodel.nv) stateReg = crocoddyl.CostModelState( self.state, crocoddyl.ActivationModelWeightedQuad( np.matrix(stateWeights**2).T), self.rmodel.defaultState, self.actuation.nu) ctrlReg = crocoddyl.CostModelControl(self.state, self.actuation.nu) costModel.addCost("stateReg", stateReg, 1e1) costModel.addCost("ctrlReg", ctrlReg, 1e-3) for i in swingFootTask: vref = crocoddyl.FrameMotion(i.frame, pinocchio.Motion.Zero()) impactFootVelCost = crocoddyl.CostModelFrameVelocity( self.state, vref, self.actuation.nu) costModel.addCost('impactVel_' + str(i.frame), impactFootVelCost, 1e6) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics( self.state, self.actuation, contactModel, costModel) model = crocoddyl.IntegratedActionModelEuler(dmodel, 0.) return model
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.contactSum = crocoddyl.ContactModelMultiple(self.ROBOT_STATE) self.datas = collections.OrderedDict( [[name, contact.createData(self.robot_data)] for name, contact in self.CONTACTS.items()]) for name, contact in self.CONTACTS.items(): self.contactSum.addContact(name, contact) self.dataSum = self.contactSum.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))
import pinocchio import example_robot_data import numpy as np from test_utils import NUMDIFF_MODIFIER, assertNumDiff crocoddyl.switchToNumpyMatrix() # Create robot model and data ROBOT_MODEL = example_robot_data.loadICub().model ROBOT_DATA = ROBOT_MODEL.createData() # Create differential action model and data; link the contact data ROBOT_STATE = crocoddyl.StateMultibody(ROBOT_MODEL) ACTUATION = crocoddyl.ActuationModelFloatingBase(ROBOT_STATE) CONTACTS = crocoddyl.ContactModelMultiple(ROBOT_STATE, ACTUATION.nu) CONTACT_6D_1 = crocoddyl.ContactModel6D( ROBOT_STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('r_sole'), pinocchio.SE3.Random()), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACT_6D_2 = crocoddyl.ContactModel6D( ROBOT_STATE, crocoddyl.FramePlacement(ROBOT_MODEL.getFrameId('l_sole'), pinocchio.SE3.Random()), ACTUATION.nu, pinocchio.utils.rand(2)) CONTACTS.addContact("r_sole_contact", CONTACT_6D_1) CONTACTS.addContact("l_sole_contact", CONTACT_6D_2) COSTS = crocoddyl.CostModelSum(ROBOT_STATE, ACTUATION.nu, False) COSTS.addCost( "force",
lfPos0 = rdata.oMf[leftFootId].translation refGripper = rdata.oMf[rmodel.getFrameId("gripper_left_joint")].translation comRef = (rfPos0 + lfPos0) / 2 comRef[2] = pinocchio.centerOfMass(rmodel, rdata, q0)[2].item() # Initialize Gepetto viewer if WITHDISPLAY: display = crocoddyl.GepettoDisplay(robot, frameNames=[rightFoot, leftFoot]) display.robot.viewer.gui.addSphere( 'world/point', .05, [1., 0., 0., 1.]) # radius = .1, RGBA=1001 display.robot.viewer.gui.applyConfiguration( 'world/point', target.tolist() + [0., 0., 0., 1.]) # xyz+quaternion # Add contact to the model contactModel = crocoddyl.ContactModelMultiple(state, actuation.nu) supportContactModelLeft = crocoddyl.ContactModel6D(state, leftFootId, pinocchio.SE3.Identity(), actuation.nu, np.array([0, 0])) contactModel.addContact(leftFoot + "_contact", supportContactModelLeft) supportContactModelRight = crocoddyl.ContactModel6D(state, rightFootId, pinocchio.SE3.Identity(), actuation.nu, np.array([0, 0])) contactModel.addContact(rightFoot + "_contact", supportContactModelRight) contactData = contactModel.createData(rdata) # Cost for self-collision maxfloat = sys.float_info.max xlb = np.concatenate([
def createSwingFootModel(self, timeStep, supportFootIds, comTask=None, swingFootTask=None): """ Action model for a swing foot phase. :param timeStep: step duration of the action model :param supportFootIds: Ids of the constrained feet :param comTask: CoM task :param swingFootTask: swinging foot task :return action model for a swing foot phase """ # Creating a 6D multi-contact model, and then including the supporting # foot nu = self.actuation.nu contactModel = crocoddyl.ContactModelMultiple(self.state, nu) for i in supportFootIds: supportContactModel = \ crocoddyl.ContactModel6D(self.state, i, pinocchio.SE3.Identity(), nu, np.array([0., 0.])) contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, nu) if isinstance(comTask, np.ndarray): comResidual = crocoddyl.ResidualModelCoMPosition( self.state, comTask, nu) comTrack = crocoddyl.CostModelResidual(self.state, comResidual) costModel.addCost("comTrack", comTrack, 1e6) for i in supportFootIds: cone = crocoddyl.WrenchCone(self.Rsurf, self.mu, np.array([0.1, 0.05])) wrenchResidual = crocoddyl.ResidualModelContactWrenchCone( self.state, i, cone, nu) wrenchActivation = crocoddyl.ActivationModelQuadraticBarrier( crocoddyl.ActivationBounds(cone.lb, cone.ub)) wrenchCone = crocoddyl.CostModelResidual(self.state, wrenchActivation, wrenchResidual) costModel.addCost(self.rmodel.frames[i].name + "_wrenchCone", wrenchCone, 1e1) if swingFootTask is not None: for i in swingFootTask: framePlacementResidual = crocoddyl.ResidualModelFramePlacement( self.state, i[0], i[1], nu) footTrack = crocoddyl.CostModelResidual( self.state, framePlacementResidual) costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack", footTrack, 1e6) stateWeights = np.array([0] * 3 + [500.] * 3 + [0.01] * (self.state.nv - 6) + [10] * self.state.nv) stateResidual = crocoddyl.ResidualModelState(self.state, self.rmodel.defaultState, nu) stateActivation = crocoddyl.ActivationModelWeightedQuad( stateWeights**2) ctrlResidual = crocoddyl.ResidualModelControl(self.state, nu) stateReg = crocoddyl.CostModelResidual(self.state, stateActivation, stateResidual) ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual) costModel.addCost("stateReg", stateReg, 1e1) costModel.addCost("ctrlReg", ctrlReg, 1e-1) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics( self.state, self.actuation, contactModel, costModel, 0., True) model = crocoddyl.IntegratedActionModelEuler(dmodel, timeStep) return model
lfPos0 = rdata.oMf[leftFootId].translation refGripper = rdata.oMf[rmodel.getFrameId("gripper_left_joint")].translation comRef = (rfPos0 + lfPos0) / 2 comRef[2] = np.asscalar(pinocchio.centerOfMass(rmodel, rdata, q0)[2]) # Initialize Gepetto viewer if WITHDISPLAY: display = crocoddyl.GepettoDisplay(robot, frameNames=[rightFoot, leftFoot]) display.robot.viewer.gui.addSphere( 'world/point', .05, [1., 0., 0., 1.]) # radius = .1, RGBA=1001 display.robot.viewer.gui.applyConfiguration( 'world/point', target.tolist() + [0., 0., 0., 1.]) # xyz+quaternion # Create two contact models used along the motion contactModel1Foot = crocoddyl.ContactModelMultiple(state, actuation.nu) contactModel2Feet = crocoddyl.ContactModelMultiple(state, actuation.nu) framePlacementLeft = crocoddyl.FramePlacement(leftFootId, pinocchio.SE3.Identity()) framePlacementRight = crocoddyl.FramePlacement(rightFootId, pinocchio.SE3.Identity()) supportContactModelLeft = crocoddyl.ContactModel6D(state, framePlacementLeft, actuation.nu, np.array([0, 40])) supportContactModelRight = crocoddyl.ContactModel6D(state, framePlacementRight, actuation.nu, np.array([0, 40])) contactModel1Foot.addContact(rightFoot + "_contact", supportContactModelRight) contactModel2Feet.addContact(leftFoot + "_contact", supportContactModelLeft) contactModel2Feet.addContact(rightFoot + "_contact", supportContactModelRight)
def createSwingFootModel(self, timeStep, supportFootIds, comTask=None, swingFootTask=None): """ Action model for a swing foot phase. :param timeStep: step duration of the action model :param supportFootIds: Ids of the constrained feet :param comTask: CoM task :param swingFootTask: swinging foot task :return action model for a swing foot phase """ # Creating a 3D multi-contact model, and then including the supporting # foot nu = self.actuation.nu contactModel = crocoddyl.ContactModelMultiple(self.state, nu) for i in supportFootIds: supportContactModel = crocoddyl.ContactModel3D(self.state, i, np.array([0., 0., 0.]), nu, np.array([0., 50.])) contactModel.addContact(self.rmodel.frames[i].name + "_contact", supportContactModel) # Creating the cost model for a contact phase costModel = crocoddyl.CostModelSum(self.state, nu) if isinstance(comTask, np.ndarray): comResidual = crocoddyl.ResidualModelCoMPosition(self.state, comTask, nu) comTrack = crocoddyl.CostModelResidual(self.state, comResidual) costModel.addCost("comTrack", comTrack, 1e6) for i in supportFootIds: cone = crocoddyl.FrictionCone(self.Rsurf, self.mu, 4, False) coneResidual = crocoddyl.ResidualModelContactFrictionCone(self.state, i, cone, nu) coneActivation = crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(cone.lb, cone.ub)) frictionCone = crocoddyl.CostModelResidual(self.state, coneActivation, coneResidual) costModel.addCost(self.rmodel.frames[i].name + "_frictionCone", frictionCone, 1e1) if swingFootTask is not None: for i in swingFootTask: frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(self.state, i[0], i[1].translation, nu) footTrack = crocoddyl.CostModelResidual(self.state, frameTranslationResidual) costModel.addCost(self.rmodel.frames[i[0]].name + "_footTrack", footTrack, 1e6) stateWeights = np.array([0.] * 3 + [500.] * 3 + [0.01] * (self.rmodel.nv - 6) + [10.] * 6 + [1.] * (self.rmodel.nv - 6)) stateResidual = crocoddyl.ResidualModelState(self.state, self.rmodel.defaultState, nu) stateActivation = crocoddyl.ActivationModelWeightedQuad(stateWeights**2) ctrlResidual = crocoddyl.ResidualModelControl(self.state, nu) stateReg = crocoddyl.CostModelResidual(self.state, stateActivation, stateResidual) ctrlReg = crocoddyl.CostModelResidual(self.state, ctrlResidual) costModel.addCost("stateReg", stateReg, 1e1) costModel.addCost("ctrlReg", ctrlReg, 1e-1) lb = np.concatenate([self.state.lb[1:self.state.nv + 1], self.state.lb[-self.state.nv:]]) ub = np.concatenate([self.state.ub[1:self.state.nv + 1], self.state.ub[-self.state.nv:]]) stateBoundsResidual = crocoddyl.ResidualModelState(self.state, nu) stateBoundsActivation = crocoddyl.ActivationModelQuadraticBarrier(crocoddyl.ActivationBounds(lb, ub)) stateBounds = crocoddyl.CostModelResidual(self.state, stateBoundsActivation, stateBoundsResidual) costModel.addCost("stateBounds", stateBounds, 1e3) # Creating the action model for the KKT dynamics with simpletic Euler # integration scheme dmodel = crocoddyl.DifferentialActionModelContactFwdDynamics(self.state, self.actuation, contactModel, costModel, 0., True) if self.integrator == 'euler': model = crocoddyl.IntegratedActionModelEuler(dmodel, self.control, timeStep) elif self.integrator == 'rk4': model = crocoddyl.IntegratedActionModelRK(dmodel, self.control, crocoddyl.RKType.four, timeStep) elif self.integrator == 'rk3': model = crocoddyl.IntegratedActionModelRK(dmodel, self.control, crocoddyl.RKType.three, timeStep) elif self.integrator == 'rk2': model = crocoddyl.IntegratedActionModelRK(dmodel, self.control, crocoddyl.RKType.two, timeStep) else: model = crocoddyl.IntegratedActionModelEuler(dmodel, self.control, timeStep) return model