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 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 cost(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
M = self.rdata.oMf[self.idEff]
self.residuals = m2a(pinocchio.log(M.inverse() * self.ref).vector)
return sum(self.residuals**2)
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 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:])
pinocchio.computeAllTerms(self.pinocchio, data.pinocchio, q, v)
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
data.tauq[:] = self.actuation.calc(data.actuation, x, u)
self.contact.calc(data.contact, x)
data.K[:nv, :nv] = data.pinocchio.M
if hasattr(self.pinocchio, 'armature'):
data.K[range(nv), range(nv)] += self.pinocchio.armature.flat
data.K[nv:, :nv] = data.contact.J
data.K.T[nv:, :nv] = data.contact.J
data.Kinv = np.linalg.inv(data.K)
data.r[:nv] = data.tauq - m2a(data.pinocchio.nle)
data.r[nv:] = -data.contact.a0
data.af[:] = np.dot(data.Kinv, data.r)
data.f[:] *= -1.
# Convert force array to vector of spatial forces.
self.contact.setForces(data.contact, data.f)
data.cost = self.costs.calc(data.costs, x, u)
return data.xout, data.cost
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
# THIS FUNCTION CALLS THE FORWARD KINEMATICS
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_jacobian = getJointJacobian(model, data, i, ReferenceFrame.WORLD)
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
# Print out the placement of each joint of the kinematic tree
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_jacobian = getFrameJacobian(model, data, i, ReferenceFrame.WORLD)
def constraint(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
M = self.rdata.oMf[self.idEff]
self.eq = m2a(M.translation) - self.refEff
return self.eq.flat
def constraint(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMeff = self.refEff.inverse() * self.rdata.oMf[self.idEff]
self.eq = m2a(pinocchio.log(refMeff).vector)
return self.eq.tolist()
def compute_forces(self, compute_data=True):
'''Compute the contact forces from q, v and elastic model'''
if compute_data:
se3.forwardKinematics(self.model, self.data, self.q, self.v,
zero(self.model.nv))
se3.computeJointJacobians(self.model, self.data)
se3.updateFramePlacements(self.model, self.data)
# check collisions only if unilateral contacts are enables
# or if the contact is not active yet
for c in self.contacts:
if (self.unilateral_contacts or not c.active):
c.check_collision()
contact_changed = False
if (self.nc != np.sum([c.active for c in self.contacts])):
contact_changed = True
print("%.3f Number of active contacts changed from %d to %d." %
(self.t, self.nc, np.sum([c.active for c in self.contacts])))
self.resize_contacts()
i = 0
for c in self.contacts:
if (c.active):
self.f[3 * i:3 * i + 3] = c.compute_force(
self.unilateral_contacts)
self.p[3 * i:3 * i + 3] = c.p
self.dp[3 * i:3 * i + 3] = c.v
self.p0[3 * i:3 * i + 3] = c.p0
self.dp0[3 * i:3 * i + 3] = c.dp0
i += 1
# if(contact_changed):
# print(i, c.frame_name, 'p', c.p.T, 'p0', c.p0.T, 'f', c.f.T)
return self.f
def constraint_rightfoot(self, x, nc=0):
q, tauq, fr, fl = self.x2qf(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMr = self.refR.inverse() * self.rdata.oMf[self.idR]
self.eq[nc:nc + 6] = m2a(pinocchio.log(refMr).vector)
return self.eq[nc:nc + 6].tolist()
def init(self, q0, v0=None, reset_contact_positions=False):
self.q = q0.copy()
if (v0 is None): self.v = zero(self.NV)
else: self.v = v0.copy()
self.dv = zero(self.NV)
# reset contact position
if (reset_contact_positions):
se3.forwardKinematics(self.robot.model, self.robot.data, self.q)
se3.updateFramePlacements(self.robot.model, self.robot.data)
self.Mrf0 = self.robot.data.oMf[self.RF].copy(
) # Initial (i.e. 0-load) position of the R spring.
self.Mlf0 = self.robot.data.oMf[self.LF].copy(
) # Initial (i.e. 0-load) position of the L spring.
self.compute_f_df(self.q, self.v, compute_data=True)
self.df = zero(4)
self.com_p, self.com_v, self.com_a, self.com_j = self.robot.get_com_and_derivatives(
self.q, self.v, self.f, self.df)
self.am, self.dam, self.ddam = self.robot.get_angular_momentum_and_derivatives(
self.q, self.v, self.f, self.df, self.Krf[0, 0], self.Krf[1, 1])
self.com_s = zero(2)
self.acc_lf = zero(3)
self.acc_rf = zero(3)
self.ddf = zero(4)
def computeJacobian(self, q):
pin.forwardKinematics(self.rmodel, self.rdata, q)
pin.updateFramePlacements(self.rmodel, self.rdata)
pin.computeJointJacobians(self.rmodel, self.rdata, q)
J = pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
return J
def constraint_leftfoot(self, x, nc=0):
q = self.vq2q(a2m(x))
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMl = self.refL.inverse() * self.rdata.oMf[self.idL]
self.eq[nc:nc + 6] = m2a(pinocchio.log(refMl).vector)
return self.eq[nc:nc + 6].tolist()
def MvJtf(q, vnext, v, f):
M = pinocchio.crba(rmodel, rdata, q)
pinocchio.computeJointJacobians(rmodel, rdata, q)
pinocchio.forwardKinematics(rmodel, rdata, q)
pinocchio.updateFramePlacements(rmodel, rdata)
J = pinocchio.getFrameJacobian(rmodel, rdata, CONTACTFRAME,
pinocchio.ReferenceFrame.LOCAL)
return M * (vnext - v) - J.T * f
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:])
a = a2m(data.a)
fs = data.contact.forces
pinocchio.computeRNEADerivatives(self.pinocchio, data.pinocchio, q, v, a, fs)
pinocchio.updateFramePlacements(self.pinocchio, data.pinocchio)
# [a;-f] = K^-1 [ tau - b, -gamma ]
# [a';-f'] = -K^-1 [ K'a + b' ; J'a + gamma' ] = -K^-1 [ rnea'(q,v,a,fs) ; acc'(q,v,a) ]
# Derivative of the actuation model tau = tau(q,u)
# dtau_dq and dtau_dv are the rnea derivatives rnea'
did_dq = data.pinocchio.dtau_dq
did_dv = data.pinocchio.dtau_dv
# Derivative of the contact constraint
# da0_dq and da0_dv are the acceleration derivatives acc'
self.contact.calcDiff(data.contact, x, recalc=False)
dacc_dq = data.contact.Aq
dacc_dv = data.contact.Av
data.Kinv = np.linalg.inv(data.K)
# We separate the Kinv into the a and f rows, and the actuation and acceleration columns
da_did = -data.Kinv[:nv, :nv]
df_did = data.Kinv[nv:, :nv]
da_dacc = -data.Kinv[:nv, nv:]
df_dacc = data.Kinv[nv:, nv:]
da_dq = np.dot(da_did, did_dq) + np.dot(da_dacc, dacc_dq)
da_dv = np.dot(da_did, did_dv) + np.dot(da_dacc, dacc_dv)
da_dtau = data.Kinv[:nv, :nv] # Add this alias just to make the code clearer
df_dtau = -data.Kinv[nv:, :nv] # Add this alias just to make the code clearer
# tau is a function of x and u (typically trivial in x), whose derivatives are Ax and Au
dtau_dx = data.actuation.Ax
dtau_du = data.actuation.Au
data.Fx[:, :nv] = da_dq
data.Fx[:, nv:] = da_dv
data.Fx += np.dot(da_dtau, dtau_dx)
data.Fu[:, :] = np.dot(da_dtau, dtau_du)
data.df_dq[:, :] = np.dot(df_did, did_dq) + np.dot(df_dacc, dacc_dq)
data.df_dv[:, :] = np.dot(df_did, did_dv) + np.dot(df_dacc, dacc_dv)
data.df_dx += np.dot(df_dtau, dtau_dx)
data.df_du[:, :] = np.dot(df_dtau, dtau_du)
self.contact.setForcesDiff(data.contact, data.df_dx, data.df_du)
self.costs.calcDiff(data.costs, x, u, recalc=False)
return data.xout, data.cost
def computeJacobian(self, q):
pin.forwardKinematics(self.rmodel, self.rdata, q)
pin.updateFramePlacements(self.rmodel, self.rdata)
pin.computeJointJacobians(self.rmodel, self.rdata, q)
J = pin.getFrameJacobian(self.rmodel, self.rdata, self.ee_frame_id,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
#modify J to remove the term corresponding to the base frame orientation
J = np.hstack([J[:, :3], np.zeros((6, 4)), J[:, 6:]])
return J
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
numpy_vector_joint_vel = np.random.rand(model.njoints - 1)
# Perform the forward kinematics over the kinematic tree
forwardKinematics(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel)
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_velocity = getVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_jacobian = getJointJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_linear_vel = joint_velocity.linear
joint_angular_vel = joint_velocity.angular
joint_6v_vel = joint_velocity.vector
err = joint_6v_vel - joint_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_velocity = getFrameVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_jacobian = getFrameJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_linear_vel = frame_velocity.linear
frame_angular_vel = frame_velocity.angular
frame_6v_vel = frame_velocity.vector
err = frame_6v_vel - frame_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
def createWalkingProblem(self, x0, stepLength, stepHeight, timeStep,
stepKnots, supportKnots):
""" Create a shooting problem for a simple walking gait.
:param x0: initial state
:param stepLength: step length
:param stepHeight: step height
:param timeStep: step time for each knot
:param stepKnots: number of knots for step phases
:param supportKnots: number of knots for double support phases
:return shooting problem
"""
# Compute the current foot positions
q0 = x0[:self.state.nq]
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
rfPos0 = self.rdata.oMf[self.rfId].translation
lfPos0 = self.rdata.oMf[self.lfId].translation
comRef = (rfPos0 + lfPos0) / 2
comRef[2] = np.asscalar(
pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
# Defining the action models along the time instances
loco3dModel = []
doubleSupport = [
self.createSwingFootModel(timeStep, [self.rfId, self.lfId])
for k in range(supportKnots)
]
# Creating the action models for three steps
if self.firstStep is True:
rStep = self.createFootstepModels(comRef, [rfPos0],
0.5 * stepLength, stepHeight,
timeStep, stepKnots, [self.lfId],
[self.rfId])
self.firstStep = False
else:
rStep = self.createFootstepModels(comRef, [rfPos0], stepLength,
stepHeight, timeStep, stepKnots,
[self.lfId], [self.rfId])
lStep = self.createFootstepModels(comRef, [lfPos0], stepLength,
stepHeight, timeStep, stepKnots,
[self.rfId], [self.lfId])
# We defined the problem as:
loco3dModel += doubleSupport + rStep
loco3dModel += doubleSupport + lStep
# Rescaling the terminal weights
costs = loco3dModel[-1].differential.costs.costs.todict()
for c in costs.values():
c.weight *= timeStep
problem = crocoddyl.ShootingProblem(x0, loco3dModel[:-1],
loco3dModel[-1])
return problem
def createJumpingProblem(self, x0, jumpHeight, jumpLength, timeStep, groundKnots, flyingKnots):
q0 = x0[:self.rmodel.nq]
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
rfFootPos0 = self.rdata.oMf[self.rfFootId].translation
rhFootPos0 = self.rdata.oMf[self.rhFootId].translation
lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
df = jumpLength[2] - rfFootPos0[2]
rfFootPos0[2] = 0.
rhFootPos0[2] = 0.
lfFootPos0[2] = 0.
lhFootPos0[2] = 0.
comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
comRef[2] = np.asscalar(pinocchio.centerOfMass(self.rmodel, self.rdata, q0)[2])
loco3dModel = []
takeOff = [
self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
) for k in range(groundKnots)
]
flyingUpPhase = [
self.createSwingFootModel(
timeStep, [],
np.array([jumpLength[0], jumpLength[1], jumpLength[2] + jumpHeight]) * (k + 1) / flyingKnots + comRef)
for k in range(flyingKnots)
]
flyingDownPhase = []
for k in range(flyingKnots):
flyingDownPhase += [self.createSwingFootModel(timeStep, [])]
f0 = jumpLength
footTask = [
crocoddyl.FramePlacement(self.lfFootId, pinocchio.SE3(np.eye(3), lfFootPos0 + f0)),
crocoddyl.FramePlacement(self.rfFootId, pinocchio.SE3(np.eye(3), rfFootPos0 + f0)),
crocoddyl.FramePlacement(self.lhFootId, pinocchio.SE3(np.eye(3), lhFootPos0 + f0)),
crocoddyl.FramePlacement(self.rhFootId, pinocchio.SE3(np.eye(3), rhFootPos0 + f0))
]
landingPhase = [
self.createFootSwitchModel([self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId], footTask, False)
]
f0[2] = df
landed = [
self.createSwingFootModel(timeStep, [self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
comTask=comRef + f0) for k in range(groundKnots)
]
loco3dModel += takeOff
loco3dModel += flyingUpPhase
loco3dModel += flyingDownPhase
loco3dModel += landingPhase
loco3dModel += landed
problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
return problem
def display(robot, q):
robot.realdisplay(q)
pio.updateFramePlacements(robot.model, robot.viz.data)
robot.viewer.gui.applyConfiguration(
'world/' + robot.model.frames[-2].name,
list(pio.se3ToXYZQUAT(robot.viz.data.oMf[-2]).flat))
robot.viewer.gui.applyConfiguration(
'world/' + robot.model.frames[-1].name,
list(pio.se3ToXYZQUAT(robot.viz.data.oMf[-1]).flat))
robot.viewer.gui.refresh()
def calc(self, q):
### Add the code to recompute your cost here
pin.forwardKinematics(self.rmodel, self.rdata, q)
pin.updateFramePlacements(self.rmodel, self.rdata)
pose = self.rdata.oMf[self.ee_frame_id]
self.pos, self.ori = pose.translation, pose.rotation
self.rpy = mat2euler(self.ori)
self.r_pos = self.sel_vector[:3] * (self.pos - self.desired_pose[:3])
self.r_ori = self.sel_vector[3:] * (self.rpy - self.desired_pose[3:])
return 0.5 * np.sum(self.r_pos**2 + self.r_ori**2)
def invgeom(h = 0.2, lx = 0.1946, ly=0.16891, leg_dir = [+1,+1,-1,-1]):
#Inputs to be modified by the user
feet_position_ref = [np.array([lx, ly, 0.0191028]),np.array([lx, -ly, 0.0191028]),np.array([-lx, ly, 0.0191028]),np.array([-lx, -ly, 0.0191028])]
base_orientation_ref = pin.utils.rpyToMatrix(0,0,0)
com_ref = np.array([0,0,h])
FL_FOOT_ID = robot.model.getFrameId('FL_FOOT')
FR_FOOT_ID = robot.model.getFrameId('FR_FOOT')
HL_FOOT_ID = robot.model.getFrameId('HL_FOOT')
HR_FOOT_ID = robot.model.getFrameId('HR_FOOT')
BASE_ID = robot.model.getFrameId('base_link')
foot_ids = np.array([FL_FOOT_ID, FR_FOOT_ID, HL_FOOT_ID, HR_FOOT_ID])
q = np.array([ 0., 0.,0.235, 0. , 0. , 0. , 1. ,
0.1 , +0.8 * leg_dir[0] , -1.6 * leg_dir[0] ,
-0.1 , +0.8 * leg_dir[1], -1.6 * leg_dir[1] ,
0.1 , -0.8 * leg_dir[2] , +1.6 * leg_dir[2] ,
-0.1 , -0.8 * leg_dir[3] , +1.6 * leg_dir[3]])
dq = robot.v0.copy()
for i in range(100):
#Compute fwd kin
pin.forwardKinematics(rmodel,rdata,q,np.zeros(rmodel.nv),np.zeros(rmodel.nv))
pin.updateFramePlacements(rmodel,rdata)
### FEET
Jfeet = []
pfeet_err = []
for i_ee in range(4):
idx = int(foot_ids[i_ee])
pos = rdata.oMf[idx].translation
ref = feet_position_ref[i_ee]
Jfeet.append(pin.computeFrameJacobian(robot.model,robot.data,q,idx,pin.LOCAL_WORLD_ALIGNED)[:3])
pfeet_err.append(ref-pos)
### CoM
com = robot.com(q)
Jcom = robot.Jcom(q)
e_com = com_ref-com
### BASE ROTATION
idx = BASE_ID
rot = rdata.oMf[idx].rotation
rotref = base_orientation_ref
Jwbasis = pin.computeFrameJacobian(robot.model,robot.data,q,idx,pin.LOCAL_WORLD_ALIGNED)[3:]
e_basisrot = -rotref @ pin.log3(rotref.T@rot)
#All tasks
J = np.vstack(Jfeet+[Jcom,Jwbasis])
x_err = np.concatenate(pfeet_err+[e_com,e_basisrot])
residual = np.linalg.norm(x_err)
if residual<1e-5:
print (np.linalg.norm(x_err))
print ("iteration: {} residual: {}".format(i,residual))
return q
dq=np.linalg.pinv(J)@(x_err)
q=pin.integrate(robot.model,q,dq)
return q * np.nan
def createCoMProblem(self, x0, comGoTo, timeStep, numKnots):
""" Create a shooting problem for a CoM forward/backward task.
:param x0: initial state
:param comGoTo: initial CoM motion
:param timeStep: step time for each knot
:param numKnots: number of knots per each phase
:return shooting problem
"""
# Compute the current foot positions
q0 = x0[:self.rmodel.nq]
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
com0 = pinocchio.centerOfMass(self.rmodel, self.rdata, q0)
# lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
# rfFootPos0 = self.rdata.oMf[self.rfFootId].translation
# lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
# rhFootPos0 = self.rdata.oMf[self.rhFootId].translation
# Defining the action models along the time instances
comModels = []
# Creating the action model for the CoM task
comForwardModels = [
self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
) for k in range(numKnots)
]
comForwardTermModel = self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
com0 + np.matrix([comGoTo, 0., 0.]).T)
comForwardTermModel.differential.costs.costs['comTrack'].weight = 1e6
comBackwardModels = [
self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
) for k in range(numKnots)
]
comBackwardTermModel = self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
com0 + np.matrix([-comGoTo, 0., 0.]).T)
comBackwardTermModel.differential.costs.costs['comTrack'].weight = 1e6
# Adding the CoM tasks
comModels += comForwardModels + [comForwardTermModel]
comModels += comBackwardModels + [comBackwardTermModel]
# Defining the shooting problem
problem = crocoddyl.ShootingProblem(x0, comModels, comModels[-1])
return problem
def cost(self, x):
tauq, fr, fl = self.x2vars(x)
pinocchio.computeAllTerms(self.rmodel, self.rdata, self.q, self.vq)
b = self.rdata.nle
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
Jr = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idR,
LOCAL)
Jl = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idL,
LOCAL)
self.residuals = m2a(b - tauq - Jr.T * fr - Jl.T * fl)
return sum(self.residuals**2)
def dConstraint_dx(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.computeJointJacobians(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMeff = self.refEff.inverse() * self.rdata.oMf[self.idEff]
log_M = pinocchio.Jlog6(refMeff)
M_q = pinocchio.getFrameJacobian(self.rmodel, self.rdata, self.idEff,
LOCAL)
self.Jeq = log_M * M_q
return self.Jeq
def createPacingProblem(self, x0, stepLength, stepHeight, timeStep,
stepKnots, supportKnots):
""" Create a shooting problem for a simple pacing gait.
:param x0: initial state
:param stepLength: step length
:param stepHeight: step height
:param timeStep: step time for each knot
:param stepKnots: number of knots for step phases
:param supportKnots: number of knots for double support phases
:return shooting problem
"""
# Compute the current foot positions
q0 = x0[:self.rmodel.nq]
pinocchio.forwardKinematics(self.rmodel, self.rdata, q0)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
rfFootPos0 = self.rdata.oMf[self.rfFootId].translation
rhFootPos0 = self.rdata.oMf[self.rhFootId].translation
lfFootPos0 = self.rdata.oMf[self.lfFootId].translation
lhFootPos0 = self.rdata.oMf[self.lhFootId].translation
comRef = (rfFootPos0 + rhFootPos0 + lfFootPos0 + lhFootPos0) / 4
comRef[2] = pinocchio.centerOfMass(self.rmodel, self.rdata,
q0)[2].item()
# Defining the action models along the time instances
loco3dModel = []
doubleSupport = [
self.createSwingFootModel(
timeStep,
[self.lfFootId, self.rfFootId, self.lhFootId, self.rhFootId],
) for k in range(supportKnots)
]
if self.firstStep is True:
rightSteps = self.createFootstepModels(
comRef, [rfFootPos0, rhFootPos0], 0.5 * stepLength, stepHeight,
timeStep, stepKnots, [self.lfFootId, self.lhFootId],
[self.rfFootId, self.rhFootId])
self.firstStep = False
else:
rightSteps = self.createFootstepModels(
comRef, [rfFootPos0, rhFootPos0], stepLength, stepHeight,
timeStep, stepKnots, [self.lfFootId, self.lhFootId],
[self.rfFootId, self.rhFootId])
leftSteps = self.createFootstepModels(comRef, [lfFootPos0, lhFootPos0],
stepLength, stepHeight, timeStep,
stepKnots,
[self.rfFootId, self.rhFootId],
[self.lfFootId, self.lhFootId])
loco3dModel += doubleSupport + rightSteps
loco3dModel += doubleSupport + leftSteps
problem = crocoddyl.ShootingProblem(x0, loco3dModel, loco3dModel[-1])
return problem
def cost(self, x):
q = a2m(x)
pinocchio.forwardKinematics(self.rmodel, self.rdata, q)
pinocchio.updateFramePlacements(self.rmodel, self.rdata)
refMl = self.refL.inverse() * self.rdata.oMf[self.idL]
residualL = m2a(pinocchio.log(refMl).vector)
refMr = self.refR.inverse() * self.rdata.oMf[self.idR]
residualR = m2a(pinocchio.log(refMr).vector)
self.residuals = np.concatenate([residualL, residualR])
return sum(self.residuals**2)
def main():
'''
Main procedure
1 ) Build the model from an URDF file
2 ) compute forwardKinematics
3 ) compute forwardKinematics of the frames
4 ) explore data
'''
model_file = Path(__file__).absolute().parents[1].joinpath(
'urdf', 'twolinks.urdf')
model = buildModelFromUrdf(str(model_file))
# Create data required by the algorithms
data = model.createData()
# Sample a random configuration
numpy_vector_joint_pos = randomConfiguration(model)
numpy_vector_joint_vel = np.random.rand(model.njoints - 1)
numpy_vector_joint_acc = np.random.rand(model.njoints - 1)
# Calls Reverese Newton-Euler algorithm
numpy_vector_joint_torques = rnea(model, data, numpy_vector_joint_pos,
numpy_vector_joint_vel,
numpy_vector_joint_acc)
# IN WHAT FOLLOWS WE CONFIRM THAT rnea COMPUTES THE FORWARD KINEMATCS
computeJointJacobians(model, data, numpy_vector_joint_pos)
joint_number = model.njoints
for i in range(joint_number):
joint = model.joints[i]
joint_placement = data.oMi[i]
joint_velocity = getVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
joint_jacobian = getJointJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
err = joint_velocity.vector - \
joint_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
# CAUTION updateFramePlacements must be called to update frame's positions
# Remark: in pinocchio frames, joints and bodies are different things
updateFramePlacements(model, data)
frame_number = model.nframes
for i in range(frame_number):
frame = model.frames[i]
frame_placement = data.oMf[i]
frame_velocity = getFrameVelocity(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
frame_jacobian = getFrameJacobian(model, data, i,
ReferenceFrame.LOCAL_WORLD_ALIGNED)
err = frame_velocity.vector - \
frame_jacobian.dot(numpy_vector_joint_vel)
assert np.linalg.norm(err, ord=np.inf) < 1.0e-10, err
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 setUp(self):
self.x = self.ROBOT_STATE.rand()
self.robot_data = self.ROBOT_MODEL.createData()
self.data = self.IMPULSE.createData(self.robot_data)
self.data_der = self.IMPULSE_DER.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))
