class StatePinocchioTest(StateTestCase):
# NUMDIFF_MODIFIER: threshold was 2.11e-4, is now 2.11e-4 (see assertNumDiff.__doc__)
# Loading Talos arm
from crocoddyl import loadTalosArm
rmodel = loadTalosArm().model
StateTestCase.NX = rmodel.nq + rmodel.nv
StateTestCase.STATE = StatePinocchio(rmodel)
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)
import numpy as np
import pinocchio
from crocoddyl import (ActionModelImpact, ActionModelNumDiff,
CostModelImpactCoM, CostModelSum, ImpulseModel6D,
ImpulseModelMultiple, a2m, loadTalosArm, loadTalosLegs)
from crocoddyl.impact import CostModelImpactWholeBody
from numpy.linalg import norm
from pinocchio.utils import zero
from testutils import NUMDIFF_MODIFIER, assertNumDiff, df_dq
# --- TALOS ARM
robot = loadTalosArm(freeFloating=False)
rmodel = robot.model
opPointName = 'root_joint'
contactName = 'arm_left_7_joint'
contactName = 'gripper_left_fingertip_1_link'
# opPointName,contactName = contactName,opPointName
CONTACTFRAME = rmodel.getFrameId(contactName)
OPPOINTFRAME = rmodel.getFrameId(opPointName)
impulseModel = ImpulseModel6D(rmodel, rmodel.getFrameId(contactName))
costModel = CostModelImpactWholeBody(rmodel)
model = ActionModelImpact(rmodel, impulseModel, costModel)
model.impulseWeight = 1.
data = model.createData()
x = model.State.rand()
# x[7:13] = 0
q = a2m(x[:model.nq])
import numpy as np
import pinocchio
from crocoddyl import loadTalosArm
from crocoddyl.utils import EPS
from testutils import NUMDIFF_MODIFIER, assertNumDiff, df_dq
robot = loadTalosArm()
rmodel = robot.model
rdata = rmodel.createData()
jid = rmodel.getJointId('gripper_left_joint')
q0 = pinocchio.randomConfiguration(rmodel)
Mref = pinocchio.SE3.Random()
def residualrMi(q):
pinocchio.forwardKinematics(rmodel, rdata, q)
return pinocchio.log(Mref.inverse() * rdata.oMi[jid]).vector
def dresidualLocal(q):
pinocchio.forwardKinematics(rmodel, rdata, q)
pinocchio.computeJointJacobians(rmodel, rdata, q)
rMi = Mref.inverse() * rdata.oMi[jid]
return np.dot(
pinocchio.Jlog6(rMi),
pinocchio.getJointJacobian(rmodel, rdata, jid,
pinocchio.ReferenceFrame.LOCAL))
class TalosArmFloatingTest(RobotTestCase):
RobotTestCase.ROBOT = loadTalosArm(freeFloating=True)
RobotTestCase.NQ = 14
RobotTestCase.NV = 13
class TalosArmTest(RobotTestCase):
RobotTestCase.ROBOT = loadTalosArm()
RobotTestCase.NQ = 7
RobotTestCase.NV = 7
self.F = np.zeros([nout, ndx + nu])
self.costResiduals = self.costs.residuals
self.Fx = self.F[:, :ndx]
self.Fu = self.F[:, -nu:]
self.Lx = self.costs.Lx
self.Lu = self.costs.Lu
self.Lxx = self.costs.Lxx
self.Lxu = self.costs.Lxu
self.Luu = self.costs.Luu
self.Rx = self.costs.Rx
self.Ru = self.costs.Ru
# Loading Talos arm with FF TODO use a bided or quadruped
# -----------------------------------------------------------------------------
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()