def test_nle(self):
model = self.model
tau = pin.nonLinearEffects(model, self.data, self.q, self.v)
data2 = model.createData()
tau_ref = pin.rnea(model, data2, self.q, self.v, self.a * 0)
self.assertApprox(tau, tau_ref)
def taskspace_control(self):
while self.engage:
# Reset for the Integral History
if not np.array_equal(self.desired_position,
self.prev_DesiredPosition):
self.log = []
# Read the states
self.joint_positions = self.simulator.get_state()[0]
self.joint_velocities = self.simulator.get_state()[1]
y, v = self.FK_Solver(self.joint_positions, self.joint_velocities)
# Control's Offline Term Estimates
M = pin.crba(self.model, self.data, self.joint_positions)
N = pin.nonLinearEffects(self.model, self.data,
self.joint_positions,
self.joint_velocities)
tau = (self.Lp * (self.desired_position - y) + self.Ld *
(self.desired_velocity - v) + self.Li * np.sum(self.log))
# Build Online Control Terms
J = self.Jacobian_Solver(self.joint_positions)
DJ = J # Naive Method as Pinocchio for DJ is not stable.
JM = M @ linalg.pinv(J)
# Compute & Send Torques
self.TAU = (JM @ tau) + N - 2 * (JM @ DJ @ self.joint_velocities)
self.clipped_TAU = np.clip(self.TAU, -10, 10)
self.simulator.send_joint_torque(self.clipped_TAU)
self.simulator.step()
# PID: Integral Anti-Windup
error = self.desired_position - y
if not np.array_equal(self.TAU, self.clipped_TAU):
sign_tau = np.sign(self.TAU)
sign_error = np.sign(error)
for i in range(sign_tau.shape[0]):
if sign_tau[i] == sign_error[i]:
self.Li = 0
break
else:
self.Li = self.Li_copy
self.log.append(error)
# Store the Desired Position
self.prev_DesiredPosition = self.desired_position
sleep(self.freq)
def joint_control(self):
while self.engage:
# Reset for the Integral History
if not np.array_equal(self.desired_position,
self.prev_DesiredPosition):
self.log = []
# Read the states
self.joint_positions = self.simulator.get_state()[0]
self.joint_velocities = self.simulator.get_state()[1]
# Control's Offline Term Estimates
M = pin.crba(self.model, self.data, self.joint_positions)
N = pin.nonLinearEffects(self.model, self.data,
self.joint_positions,
self.joint_velocities)
tau = (self.Kp * (self.desired_position - self.joint_positions) +
self.Kd * (self.desired_velocity - self.joint_velocities) +
self.Ki * np.sum(self.log))
self.TAU = M @ tau + N
self.clipped_TAU = np.clip(self.TAU, -10, 10)
self.simulator.send_joint_torque(self.clipped_TAU)
self.simulator.step()
# PID: Integral Anti-Windup
error = self.desired_position - self.joint_positions
if not np.array_equal(self.TAU, self.clipped_TAU):
sign_tau = np.sign(self.TAU)
sign_error = np.sign(error)
for i in range(sign_tau.shape[0]):
if sign_tau[i] == sign_error[i]:
self.Ki = 0
break
else:
self.Ki = self.Ki_copy
self.log.append(error)
# Store the Desired Position
self.prev_DesiredPosition = self.desired_position
sleep(self.freq)
def update_quantities(jiminy_model,
position,
velocity=None,
acceleration=None,
update_physics=True,
update_com=False,
update_energy=False,
update_jacobian=False,
use_theoretical_model=True):
"""
@brief Compute all quantities using position, velocity and acceleration
configurations.
@details Run multiple algorithms to compute all quantities
which can be known with the model position, velocity
and acceleration configuration.
This includes:
- body spatial transforms,
- body spatial velocities,
- body spatial drifts,
- body transform acceleration,
- body transform jacobians,
- center-of-mass position,
- center-of-mass velocity,
- center-of-mass drift,
- center-of-mass acceleration,
(- center-of-mass jacobian : No Python binding available so far),
- articular inertia matrix,
- non-linear effects (Coriolis + gravity)
Computation results are stored in internal
data and can be retrieved with associated getters.
@note This function modifies internal data.
@param jiminy_model The Jiminy model
@param position Joint position vector
@param velocity Joint velocity vector
@param acceleration Joint acceleration vector
@pre model_.nq == positionIn.size()
@pre model_.nv == velocityIn.size()
@pre model_.nv == accelerationIn.size()
"""
if use_theoretical_model:
pnc_model = jiminy_model.pinocchio_model_th
pnc_data = jiminy_model.pinocchio_data_th
else:
pnc_model = jiminy_model.pinocchio_model
pnc_data = jiminy_model.pinocchio_data
if (update_physics and update_com and \
update_energy and update_jacobian and \
velocity is not None):
pnc.computeAllTerms(pnc_model, pnc_data, position, velocity)
else:
if update_physics:
if velocity is not None:
pnc.nonLinearEffects(pnc_model, pnc_data, position, velocity)
pnc.crba(pnc_model, pnc_data, position)
if update_jacobian:
# if update_com:
# pnc.getJacobianComFromCrba(pnc_model, pnc_data)
pnc.computeJointJacobians(pnc_model, pnc_data)
if update_com:
if velocity is None:
pnc.centerOfMass(pnc_model, pnc_data, position)
elif acceleration is None:
pnc.centerOfMass(pnc_model, pnc_data, position, velocity)
else:
pnc.centerOfMass(pnc_model, pnc_data, position, velocity,
acceleration)
else:
if velocity is None:
pnc.forwardKinematics(pnc_model, pnc_data, position)
elif acceleration is None:
pnc.forwardKinematics(pnc_model, pnc_data, position, velocity)
else:
pnc.forwardKinematics(pnc_model, pnc_data, position, velocity,
acceleration)
pnc.framesForwardKinematics(pnc_model, pnc_data, position)
if update_energy:
if velocity is not None:
pnc.kineticEnergy(pnc_model, pnc_data, position, velocity,
False)
pnc.potentialEnergy(pnc_model, pnc_data, position, False)
def get_coriolis(self):
return np.copy(
pin.nonLinearEffects(self._model, self._data, self._q, self._q_dot)
- self.get_gravity())
def update_quantities(robot: jiminy.Model,
position: np.ndarray,
velocity: Optional[np.ndarray] = None,
acceleration: Optional[np.ndarray] = None,
update_physics: bool = True,
update_com: bool = True,
update_energy: bool = True,
update_jacobian: bool = False,
update_collisions: bool = True,
use_theoretical_model: bool = True) -> None:
"""Compute all quantities using position, velocity and acceleration
configurations.
Run multiple algorithms to compute all quantities which can be known with
the model position, velocity and acceleration.
This includes:
- body spatial transforms,
- body spatial velocities,
- body spatial drifts,
- body transform acceleration,
- body transform jacobians,
- center-of-mass position,
- center-of-mass velocity,
- center-of-mass drift,
- center-of-mass acceleration,
- center-of-mass jacobian,
- articular inertia matrix,
- non-linear effects (Coriolis + gravity)
- collisions and distances
.. note::
Computation results are stored internally in the robot, and can
be retrieved with associated getters.
.. warning::
This function modifies the internal robot data.
.. warning::
It does not called overloaded pinocchio methods provided by
`jiminy_py.core` but the original pinocchio methods instead. As a
result, it does not take into account the rotor inertias / armatures.
One is responsible of calling the appropriate methods manually instead
of this one if necessary. This behavior is expected to change in the
future.
:param robot: Jiminy robot.
:param position: Robot position vector.
:param velocity: Robot velocity vector.
:param acceleration: Robot acceleration vector.
:param update_physics: Whether or not to compute the non-linear effects and
internal/external forces.
Optional: True by default.
:param update_com: Whether or not to compute the COM of the robot AND each
link individually. The global COM is the first index.
Optional: False by default.
:param update_energy: Whether or not to compute the energy of the robot.
Optional: False by default
:param update_jacobian: Whether or not to compute the jacobians.
Optional: False by default.
:param use_theoretical_model: Whether the state corresponds to the
theoretical model when updating and fetching
the robot's state.
Optional: True by default.
"""
if use_theoretical_model:
pnc_model = robot.pinocchio_model_th
pnc_data = robot.pinocchio_data_th
else:
pnc_model = robot.pinocchio_model
pnc_data = robot.pinocchio_data
if (update_physics and update_com and
update_energy and update_jacobian and
velocity is not None and acceleration is None):
pin.computeAllTerms(pnc_model, pnc_data, position, velocity)
else:
if velocity is None:
pin.forwardKinematics(pnc_model, pnc_data, position)
elif acceleration is None:
pin.forwardKinematics(pnc_model, pnc_data, position, velocity)
else:
pin.forwardKinematics(
pnc_model, pnc_data, position, velocity, acceleration)
if update_com:
if velocity is None:
pin.centerOfMass(pnc_model, pnc_data, position)
elif acceleration is None:
pin.centerOfMass(pnc_model, pnc_data, position, velocity)
else:
pin.centerOfMass(
pnc_model, pnc_data, position, velocity, acceleration)
if update_jacobian:
if update_com:
pin.jacobianCenterOfMass(pnc_model, pnc_data)
pin.computeJointJacobians(pnc_model, pnc_data)
if update_physics:
if velocity is not None:
pin.nonLinearEffects(pnc_model, pnc_data, position, velocity)
pin.crba(pnc_model, pnc_data, position)
if update_energy:
if velocity is not None:
pin.computeKineticEnergy(pnc_model, pnc_data)
pin.computePotentialEnergy(pnc_model, pnc_data)
pin.updateFramePlacements(pnc_model, pnc_data)
if update_collisions:
pin.updateGeometryPlacements(
pnc_model, pnc_data, robot.collision_model, robot.collision_data)
pin.computeCollisions(
robot.collision_model, robot.collision_data,
stop_at_first_collision=False)
pin.computeDistances(robot.collision_model, robot.collision_data)
for dist_req in robot.collision_data.distanceResults:
if np.linalg.norm(dist_req.normal) < 1e-6:
pin.computeDistances(
robot.collision_model, robot.collision_data)
break
def compute_inverse_dynamics(robot: jiminy.Model,
position: np.ndarray,
velocity: np.ndarray,
acceleration: np.ndarray,
use_theoretical_model: bool = False
) -> np.ndarray:
"""Compute the motor torques through inverse dynamics, assuming to external
forces except the one resulting from the anyaltical constraints applied on
the model.
.. warning::
This function modifies the internal robot data.
:param robot: Jiminy robot.
:param position: Robot configuration vector.
:param velocity: Robot velocity vector.
:param acceleration: Robot acceleration vector.
:param use_theoretical_model: Whether the position, velocity and
acceleration are associated with the
theoretical model instead of the actual one.
Optional: False by default.
:returns: motor torques.
"""
# Convert theoretical position, velocity and acceleration if necessary
if use_theoretical_model and robot.is_flexible:
position = robot.get_flexible_configuration_from_rigid(position)
velocity = robot.get_flexible_velocity_from_rigid(velocity)
acceleration = robot.get_flexible_velocity_from_rigid(acceleration)
# Define some proxies for convenience
pnc_model = robot.pinocchio_model
pnc_data = robot.pinocchio_data
motors_velocity_idx = robot.motors_velocity_idx
# Updating kinematics quantities
pin.forwardKinematics(
pnc_model, pnc_data, position, velocity, acceleration)
pin.updateFramePlacements(pnc_model, pnc_data)
# Compute inverted inertia matrix, taking into account rotor inertias.
# Note that `pin.computeMinverse` must NOT be used, since it does not take
# into account those inertias.
M = jiminy.crba(pnc_model, pnc_data, position)
M_inv = np.linalg.inv(M)
# Compute non-linear effects
pin.nonLinearEffects(pnc_model, pnc_data, position, velocity)
nle = robot.pinocchio_data.nle
# Compute constraint jacobian and drift
robot.compute_constraints(position, velocity)
J = robot.get_constraints_jacobian()
drift = robot.get_constraints_drift()
# Compute constraint forces
inv_term = np.linalg.inv(J @ M_inv @ J.T)
a_f = inv_term @ (- drift + J @ M_inv @ nle)
B_f = (- inv_term @ (J @ M_inv[:, motors_velocity_idx]))
# compute feedforward term
a_ydd = (M_inv @ (- nle + J.T @ a_f) - acceleration)[motors_velocity_idx]
B_ydd = (
M_inv[:, motors_velocity_idx] + M_inv @ J.T @ B_f)[motors_velocity_idx]
# Moore-Penrose pseudo-inverse of B_ydd
B_ydd_inverse = np.linalg.pinv(B_ydd)
# Compute motor torques
u = - B_ydd_inverse @ a_ydd
return u
def step(self,
inner_step,
u,
dt=None,
use_exponential_integrator=True,
dt_force_pred=None,
ndt_force_pred=None,
update_expm=True):
if dt is None:
dt = self.dt
if self.first_iter:
self.compute_forces()
self.first_iter = False
# 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)
se3.crba(self.model, self.data, self.q)
se3.nonLinearEffects(self.model, self.data, self.q, self.v)
i = 0
for c in self.contacts:
if (c.active):
self.Jc[3 * i:3 * i + 3, :] = c.getJacobianWorldFrame()
c.f_inner[:, inner_step] = self.f[3 * i:3 * i + 3]
i += 1
# array containing the forces predicting during the time step (meaningful only for exponential integrator)
if (dt_force_pred is not None):
f_pred = np.empty((self.nk, ndt_force_pred)) * nan
for c in self.contacts:
c.f_pred = zero((3, ndt_force_pred))
c.f_avg = zero((3, ndt_force_pred))
c.f_avg2 = zero((3, ndt_force_pred))
else:
f_pred = None
f_pred_int = None
if (not use_exponential_integrator):
self.dv = self.forward_dyn(self.S.T @ u + self.Jc.T @ self.f)
v_mean = self.v + 0.5 * dt * self.dv
self.v += self.dv * dt
self.q = se3.integrate(self.model, self.q, v_mean * dt)
else:
x0, dv_bar, JMinv = self.compute_exponential_LDS(u, update_expm)
# Sanity check on contact point Jacobian and dJv
# self.compute_dJv_finite_difference()
# USE THE VALUE COMPUTED WITH FINITE DIFFERENCING FOR NOW
# self.dJv = np.copy(self.debug_dJv_fd)
# self.a[self.nk:] = JMinv@(self.S.T@u-h) + self.dJv
self.logToFile(x0)
if (update_expm):
if (self.unilateral_contacts == 'QP'):
self.int_exp_A = compute_integral_expm(self.A, dt)
self.dp0_qp = self.solve_friction_QP(x0, dt)
x0[self.nk:] -= self.dp0_qp
int_x = self.expMatHelper.compute_integral_x_T(
self.A, self.a, x0, dt, self.max_mat_mult)
# store int_x because it may be needed to compute int2_x without updating expm in next iteration
self.int_x_prev = int_x
else:
int_x = self.expMatHelper.compute_next_integral()
# compute average force
self.f_avg = self.D @ int_x / dt
# project average forces in friction cones
self.f_avg_pre_projection = np.copy(self.f_avg)
i = 0
for c in self.contacts:
if (c.active):
if (self.unilateral_contacts == 'projection'):
self.f_avg[3 * i:3 * i + 3] = c.project_force_in_cone(
self.f_avg[3 * i:3 * i + 3])
c.f_avg[:, inner_step] = self.f_avg[3 * i:3 * i + 3]
i += 1
dv_mean = dv_bar + JMinv.T @ self.f_avg
if (self.use_second_integral):
if (update_expm):
int2_x = self.expMatHelper.compute_double_integral_x_T(
self.A, self.a, x0, dt, self.max_mat_mult)
else:
int2_x = self.expMatHelper.compute_next_double_integral()
int2_x -= dt * self.int_x_prev
self.int_x_prev += int_x
self.f_avg2 = self.D @ int2_x / (0.5 * dt * dt)
# project average forces in friction cones
self.f_avg2_pre_projection = np.copy(self.f_avg2)
i = 0
for c in self.contacts:
if (c.active):
if (self.unilateral_contacts == 'projection'):
self.f_avg2[3 * i:3 * i +
3] = c.project_force_in_cone(
self.f_avg2[3 * i:3 * i + 3])
c.f_avg2[:, inner_step] = self.f_avg2[3 * i:3 * i + 3]
i += 1
v_mean = self.v + 0.5 * dt * (dv_bar + JMinv.T @ self.f_avg2)
else:
v_mean = self.v + 0.5 * dt * dv_mean
if (dt_force_pred is not None):
# predict intermediate forces using linear dynamical system (i.e. matrix exponential)
n = self.A.shape[0]
C = np.zeros((n + 1, n + 1))
C[0:n, 0:n] = self.A
C[0:n, n] = self.a
z = np.concatenate((x0, [1.0]))
e_TC = expm(dt_force_pred / ndt_force_pred * C)
for i in range(ndt_force_pred):
f_pred[:, i] = self.D @ z[:n]
z = e_TC @ z
i = 0
for c in self.contacts:
if (c.active):
c.f_pred = f_pred[3 * i:3 * i + 3, :]
i += 1
# predict also what forces we would get by integrating with the force prediction
int_x = self.expMatHelper.compute_integral_x_T(
self.A,
self.a,
x0,
dt_force_pred,
self.max_mat_mult,
store=False)
int2_x = self.expMatHelper.compute_double_integral_x_T(
self.A,
self.a,
x0,
dt_force_pred,
self.max_mat_mult,
store=False)
D_int_x = self.D @ int_x
D_int2_x = self.D @ int2_x
v_mean_pred = self.v + 0.5 * dt_force_pred * dv_bar + JMinv.T @ D_int2_x / dt_force_pred
dv_mean_pred = dv_bar + JMinv.T @ D_int_x / dt_force_pred
v_pred = self.v + dt_force_pred * dv_mean_pred
q_pred = se3.integrate(self.model, self.q,
v_mean_pred * dt_force_pred)
se3.forwardKinematics(self.model, self.data, q_pred, v_pred)
se3.updateFramePlacements(self.model, self.data)
f_pred_int = np.zeros(self.nk)
# comment these lines because they were messing up the anchor point updates
# for (i,c) in enumerate(self.contacts):
# f_pred_int[3*i:3*i+3] = c.compute_force(self.unilateral_contacts)
# DEBUG: predict forces integrating contact point dynamics while updating robot dynamics M and h
# t = dt_force_pred/ndt_force_pred
# f_pred[:, 0] = K_p0 + self.D @ x0
# x = np.copy(x0)
# q, v = np.copy(self.q), np.copy(self.v)
# for i in range(1,ndt_force_pred):
# # integrate robot state
# dv = np.linalg.solve(M, self.S.T@u - h + self.Jc.T@f_pred[:,i-1])
# v_tmp = v + 0.5*dt*dv
# v += dv*dt
# q = se3.integrate(self.model, q, v_tmp*dt)
# # update kinematics
# se3.forwardKinematics(self.model, self.data, q, v, np.zeros(self.model.nv))
# se3.computeJointJacobians(self.model, self.data)
# se3.updateFramePlacements(self.model, self.data)
# ii = 0
# for c in self.contacts:
# J = c.getJacobianWorldFrame()
# self.Jc[ii:ii+3, :] = J
# self.p[i:i+3] = c.p
# self.dp[i:i+3] = c.v
# self.dJv[i:i+3] = c.dJv
# ii += 3
# x0 = np.concatenate((self.p, self.dp))
# # update M and h
# se3.crba(self.model, self.data, q)
# se3.nonLinearEffects(self.model, self.data, q, v)
# M = self.data.M
# h = self.data.nle
# # recompute A and a
# JMinv = np.linalg.solve(M, self.Jc.T).T
# self.Upsilon = [email protected]
# self.a[self.nk:] = JMinv@(self.S.T@u - h) + self.dJv + self.Upsilon@K_p0
# self.A[self.nk:, :self.nk] = [email protected]
# self.A[self.nk:, self.nk:] = [email protected]
# # integrate LDS
# dx = self.A @ x + self.a
# x += t * dx
# f_pred[:, i] = f_pred[:,i-1] + t*self.D @ dx
# DEBUG: predict forces integrating contact point dynamics with Euler
# t = dt_force_pred/ndt_force_pred
# f_pred[:, 0] = K_p0 + self.D @ x0
# x = np.copy(x0)
# for i in range(1,ndt_force_pred):
# dx = self.A @ x + self.a
# x += t * dx
# f_pred[:, i] = f_pred[:,i-1] + t*self.D @ dx
# DEBUG: predict forces assuming constant contact point acceleration (works really bad)
# df_debug = self.D @ (self.A @ x0 + self.a)
# dv = np.linalg.solve(M, self.S.T@u - h + [email protected])
# ddp = self.Jc @ dv + self.dJv
# df = -self.K @ self.dp - self.B @ ddp
# if(norm(df - df_debug)>1e-6):
# print("Error:", norm(df - df_debug))
# f0 = K_p0 + self.D @ x0
# t = 0.0
# for i in range(ndt_force_pred):
# f_pred[:, i] = f0 + t*df
# t += dt_force_pred/ndt_force_pred
# DEBUG: predict forces assuming constant contact point velocity (works reasonably well)
# df = -self.K @ self.dp
# f0 = K_p0 + self.D @ x0
# t = 0.0
# for i in range(ndt_force_pred):
# f_pred[:, i] = f0 + t*df
# t += dt_force_pred/ndt_force_pred
self.v += dt * dv_mean
self.q = se3.integrate(self.model, self.q, v_mean * dt)
self.dv = dv_mean
# compute forces at the end so that user has access to updated forces
self.compute_forces()
self.t += dt
return self.q, self.v, f_pred, f_pred_int
# Loading a robot model model = example_robot_data.loadHyQ().model data = model.createData() #start configuration v = np.array([0.0 , 0.0 , 0.0, 0.0, 0.0, 0.0, #underactuated 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) #actuated q = example_robot_data.loadHyQ().q0 # Update the joint and frame placements pin.forwardKinematics(model,data,q,v) pin.updateFramePlacements(model,data) M = pin.crba(model, data, q) H = pin.nonLinearEffects(model, data, q, v) G = pin.computeGeneralizedGravity(model,data, q) #EXERCISE 1: Compute the com of the robot (in WF) #..... # compare the result by using native pinocchio function com_test = pin.centerOfMass(model, data, q, v) print "Com Position (pinocchio): ", com_test # EXERCIZE 2: Compute robot kinetic energy #get a random generalized velocity v = rand(model.nv) # Update the joint and frame placements pin.forwardKinematics(model,data,q,v) # compute using generalized velocities and system mass matrix