def compute_freeflyer_state_from_fixed_body(jiminy_model,
fixed_body_name,
position,
velocity=None,
acceleration=None,
use_theoretical_model=True):
"""
@brief Fill rootjoint data from articular data when a body is fixed parallel to world.
@details The hypothesis is that 'fixed_body_name' is fixed parallel to world frame.
So this method computes the position of freeflyer rootjoint in the fixed body frame.
@note This function modifies internal data.
@param model The Jiminy model
@param fixed_body_name The name of the body that is considered fixed parallel to world frame
@param[inout] position Must contain current articular data. The rootjoint data can
contain any value, it will be ignored and replaced
The method fills in rootjoint data
@param[inout] velocity Same as positionInOut but for velocity
@pre Rootjoint must be a freeflyer
@pre positionInOut.size() == model_->nq
"""
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
position[:6].fill(0)
position[6] = 1.0
if velocity is not None:
velocity[:6].fill(0)
else:
velocity = np.zeros((pnc_model.nv, ))
if acceleration is not None:
acceleration[:6].fill(0)
else:
acceleration = np.zeros((pnc_model.nv, ))
pnc.forwardKinematics(pnc_model, pnc_data, position, velocity,
acceleration)
pnc.framesForwardKinematics(pnc_model, pnc_data, position)
ff_M_fixed_body = get_body_world_transform(jiminy_model, fixed_body_name)
w_M_ff = ff_M_fixed_body.inverse()
base_link_translation = w_M_ff.translation
base_link_quaternion = Quaternion(w_M_ff.rotation)
position[:3] = base_link_translation
position[3:7] = base_link_quaternion.coeffs()
ff_v_fixed_body = get_body_world_velocity(jiminy_model, fixed_body_name)
base_link_velocity = -ff_v_fixed_body
velocity[:6] = base_link_velocity.vector
ff_a_fixedBody = get_body_world_acceleration(jiminy_model, fixed_body_name)
base_link_acceleration = -ff_a_fixedBody
acceleration[:6] = base_link_acceleration.vector
def update_state(self, q, v):
nf, ny = self.nf, self.ny
se3.computeAllTerms(self.robot.model, self.robot.data, q, v)
se3.framesForwardKinematics(self.robot.model, self.robot.data, q) # replace with updateFramePlacements(model, data)
M = self.robot.data.M #(7,7)
Mj_diag = np.matrix(np.diag(np.diag(M[ny:,ny:])))
Jl,Jr = self.robot.get_Jl_Jr_world(q, False)
J = np.vstack([Jl[1:3],Jr[1:3]]) # (4, 7)
Mlf, Mrf = self.robot.get_Mlf_Mrf(q, False)
pyl, pzl = Mlf.translation[1:].A1
pyr, pzr = Mrf.translation[1:].A1
com, com_vel = self.robot.get_com_and_derivatives(q, v)
cy, cz = com.A1
X_am = np.matrix([-(pzl-cz),+(pyl-cy),-(pzr-cz),+(pyr-cy)])
# X_com = np.hstack([np.eye(2)/M[0,0],np.eye(2)/M[0,0]])
X_com = np.hstack([np.eye(2),np.eye(2)])
self.X = np.vstack([X_com, X_am])
self.X_pinv = np.linalg.pinv(self.X)
self.A[ ny:2*ny, 2*ny:2*ny+nf] = self.X
self.H[ ny:2*ny, 2*ny:2*ny+nf] = self.X
self.XT_Mb_inv = self.X.T * np.linalg.inv(M[:ny,:ny])
Minv = np.linalg.inv(M)
Upsilon = J*Minv*J.T
S = matlib.zeros((self.na, self.nv))
S[:,self.nv-self.na:] = matlib.eye(self.na)
JSTpinv = np.linalg.pinv(J*S.T)
A = J*Minv*S.T*Mj_diag*JSTpinv
self.K_A = self.K*A
self.K_Upsilon = self.K*Upsilon
def relativePlacement(rmodel, rdata, config, frameIndex1, frameIndex2):
pio.framesForwardKinematics(rmodel, rdata, config)
oMf1 = rdata.oMf[frameIndex1]
oMf2 = rdata.oMf[frameIndex2]
return np.linalg.inv(oMf1) @ oMf2
def _inverse_kinematics_step(
self, frame_id: int, xdes: np.ndarray,
q0: np.ndarray) -> typing.Tuple[np.ndarray, np.ndarray]:
"""Compute one IK iteration for a single finger."""
dt = 1.0e-1
pinocchio.computeJointJacobians(
self.robot_model,
self.data,
q0,
)
pinocchio.framesForwardKinematics(
self.robot_model,
self.data,
q0,
)
Ji = pinocchio.getFrameJacobian(
self.robot_model,
self.data,
frame_id,
pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED,
)[:3, :]
xcurrent = self.data.oMf[frame_id].translation
try:
Jinv = np.linalg.inv(Ji)
except Exception:
Jinv = np.linalg.pinv(Ji)
err = xdes - xcurrent
dq = Jinv.dot(xdes - xcurrent)
qnext = pinocchio.integrate(self.robot_model, q0, dt * dq)
return qnext, err
def display(self, q, force):
"""Display the robot at configuration q in the viewer by placing all the bodies."""
gui = self.gui
pin.framesForwardKinematics(self.model,self.data,q)
if self.display_visuals:
pin.updateGeometryPlacements(self.model, self.data, self.visual_model, self.visual_data)
gui.applyConfigurations (
[ self.getViewerNodeName(visual,pin.GeometryType.VISUAL) for visual in self.visual_model.geometryObjects ],
[ pin.SE3ToXYZQUATtuple(self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]) for visual in self.visual_model.geometryObjects ]
)
for contactIndex, contactName in enumerate(self.contactNames):
forceVector = np.zeros(3)
if(self.type=="Biped"):
for i in range(4):
forceVector += force[:,4*contactIndex+i]
elif(self.type=="Quadruped"):
forceVector =force[:,contactIndex]
forceNorm = np.linalg.norm(forceVector)
if forceNorm<=1.e-3:
forceVisiblity = "OFF"
else:
forceVisiblity = "ON"
self.displayContact(contactName, forceVector, forceVisiblity)
gui.refresh()
def calcDiff(self, q):
pin.framesForwardKinematics(self.rmodel, self.rdata, q)
pin.computeJointJacobians(self.rmodel, self.rdata, q)
M = self.rdata.oMf[self.frameIndex]
J = pin.getFrameJacobian(self.rmodel, self.rdata, self.frameIndex,
pin.LOCAL_WORLD_ALIGNED)[:3, :]
return 2 * J.T @ (M.translation - self.ptarget)
def getForwardKinematics(self, q=None):
if q is None:
q = self.current_j_pos
if q.shape[0] == 7:
q_tmp = q.copy()
q = np.zeros(9)
q[:7] = q_tmp
pinocchio.framesForwardKinematics(self.model, self.data, q)
def callback(self, q):
if self.viz is None:
return
pin.framesForwardKinematics(self.rmodel, self.rdata, q)
M = self.rdata.oMf[self.frameIndex]
vizutils.applyViewerConfiguration(self.viz, 'world/ball',
pin.SE3ToXYZQUATtuple(M))
vizutils.applyViewerConfiguration(self.viz, 'world/box',
self.ptarget.tolist() + [1, 0, 0, 0])
self.viz.display(q)
time.sleep(1e-2)
def forward_kinematics(joint_positions):
"""compute the forward kinematics"""
pinocchio.framesForwardKinematics(model, data, joint_positions)
# get the position of the tip link
pos = data.oMf[tip_link_id].translation
# convert from np.matrix to a flat array (for easy printing)
pos = np.asarray(pos).reshape(-1)
return pos
def getJacobian(self, q=None):
if q is None:
q = self.current_j_pos
if q.shape[0] == 7:
q_tmp = q.copy()
q = np.zeros(9)
q[:7] = q_tmp
pinocchio.computeJointJacobians(self.model, self.data, q)
pinocchio.framesForwardKinematics(self.model, self.data, q)
return pinocchio.getFrameJacobian(self.model, self.data,
self.end_effector_frame_id,
pinocchio.LOCAL_WORLD_ALIGNED)[:, :7]
def compute_closed_loop_transition_matrix(gains_array, robot, q, v, update=True):
gains = Gains(gains_array)
ny=3
H = matlib.zeros((3*nf+2*ny, 3*nf+2*ny))
H[:ny, ny:2*ny] = matlib.eye(ny)
H_f = matlib.zeros((3*nf,3*nf))
H_f[ :nf, nf:2*nf] = matlib.eye(nf)
H_f[nf:2*nf, 2*nf:3*nf] = matlib.eye(nf)
H_f[2*nf:, 2*nf:3*nf] = -gains.kd_bar*matlib.eye(nf)
if(update):
se3.computeAllTerms(robot.model, robot.data, q, v)
se3.framesForwardKinematics(robot.model, robot.data, q)
M = robot.data.M #(7,7)
Jl,Jr = robot.get_Jl_Jr_world(q, False)
J = np.vstack([Jl[1:3],Jr[1:3]]) # (4, 7)
Mlf, Mrf = robot.get_Mlf_Mrf(q, False)
pyl, pzl = Mlf.translation[1:].A1
pyr, pzr = Mrf.translation[1:].A1
com, com_vel = robot.get_com_and_derivatives(q, v)
cy, cz = com.A1
X_am = np.matrix([-(pzl-cz),+(pyl-cy),-(pzr-cz),+(pyr-cy)])
X_com = np.hstack([np.eye(2),np.eye(2)])
X = np.vstack([X_com, X_am])
X_pinv = np.linalg.pinv(X)
Minv = np.linalg.inv(M)
Upsilon = J*Minv*J.T
S = matlib.zeros((na,nv))
S[:,nv-na:] = matlib.eye(na)
JSTpinv = np.linalg.pinv(J*S.T)
A = J*Minv*S.T*Mj_diag*JSTpinv
# compute closed-loop transition matrix
K1 = gains.kp_bar*K*A*gains.Kf
K2 = K*Upsilon + gains.kp_bar*matlib.eye(nf)
H_f[2*nf:, 1*nf:2*nf] = - K2
H_f[2*nf:, 0*nf:1*nf] = - K1
H[2*ny:, 2*ny:] = H_f
H[ ny:2*ny, 2*ny:2*ny+nf] = X
H[ -nf:, :ny] = -K1*X_pinv*gains.Kp_com
H[ -nf:, ny:2*ny] = -K1*X_pinv*gains.Kd_com
return H
def footInverseKinematicsFixedBase(self, foot_pos_des, frame_name):
frame_id = self.model.getFrameId(frame_name)
blockIdx = self.getBlockIndex(frame_name)
anymal_q0 = np.vstack([-0.1, 0.7, -1., -0.1, -0.7, 1., 0.1, 0.7, -1., 0.1, -0.7, 1.])
q = anymal_q0
eps = 0.005
IT_MAX = 200
DT = 1e-1
err = np.zeros((3, 1))
e = np.zeros((3, 1))
i = 0
while True:
pinocchio.forwardKinematics(self.model, self.data, q)
pinocchio.framesForwardKinematics(self.model, self.data, q)
foot_pos = self.data.oMf[frame_id].translation
# err = np.hstack([err, (foot_pos - foot_pos_des)])
# e = err[:,-1]
# print foot_pos_des[0], foot_pos[[0]], foot_pos[[0]] - foot_pos_des[0]
# e = foot_pos - foot_pos_des
e[0] = foot_pos[[0]] - foot_pos_des[0]
e[1] = foot_pos[[1]] - foot_pos_des[1]
e[2] = foot_pos[[2]] - foot_pos_des[2]
J = pinocchio.frameJacobian(self.model, self.data, q, frame_id)
J_lin = J[:3, :]
if np.linalg.norm(e) < eps:
# print("IK Convergence achieved!")
IKsuccess = True
break
if i >= IT_MAX:
print(
"\n Warning: the iterative algorithm has not reached convergence to the desired precision. Error is: ",
np.linalg.norm(e))
IKsuccess = False
break
# print J_lin
v = - np.linalg.pinv(J_lin) * e
q = pinocchio.integrate(self.model, q, v * DT)
i += 1
# print i
q_leg = q[blockIdx:blockIdx + 3]
J_leg = J_lin[:, blockIdx:blockIdx + 3]
return q_leg, J_leg, err, IKsuccess
def forward_kinematics(
self,
joint_positions: typing.List[np.ndarray],
joint_velocities: typing.Optional[np.ndarray] = None,
) -> typing.Union[typing.List[np.ndarray], typing.Tuple[
typing.List[np.ndarray], typing.List[np.ndarray]], ]:
"""Compute end-effector positions (and velocities) for the given joint configuration.
Args:
joint_positions: Flat list of angular joint positions.
joint_velocities: Optional. Flat list of angular joint
velocities.
Returns:
If only joint positions are given: List of end-effector
positions. Each position is given as an np.array with x,y,z
positions.
If joint positions and velocities are given: Tuple with
(i) list of end-effector positions and (ii) list of
end-effector velocities. Each position and velocity is given
as an np.array with x,y,z components.
"""
pinocchio.framesForwardKinematics(self.robot_model, self.data,
joint_positions)
positions = [
np.asarray(
self.data.oMf[link_id].translation).reshape(-1).tolist()
for link_id in self.tip_link_ids
]
if joint_velocities is None:
return positions
else:
pinocchio.forwardKinematics(self.robot_model, self.data,
joint_positions, joint_velocities)
velocities = []
for link_id in self.tip_link_ids:
local_to_world_transform = pinocchio.SE3.Identity()
local_to_world_transform.rotation = self.data.oMf[
link_id].rotation
v_local = pinocchio.getFrameVelocity(self.robot_model,
self.data, link_id)
v_world = local_to_world_transform.act(v_local)
velocities.append(v_world.linear)
return positions, velocities
def TimeDerivativeJacobian(self, q, v):
"""
Description:
1. Computes the Time Derivative Jacobian Tensor of the robot.
Args:
q -> np.array (3,) : Joint Positions of the robot.
v -> np.array (3,) : Joint Velocities of the robot.
Returns:
np.array (3,3): Time Derivative Jacobian Tensor of the robot.
"""
frame = pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
pin.computeJointJacobians(self.model, self.model_data, q)
pin.framesForwardKinematics(self.model, self.model_data, q)
pin.computeJointJacobiansTimeVariation(self.model, self.model_data, q,
v)
return pin.getJointJacobianTimeVariation(self.model, self.model_data,
self.EOAT_ID, frame)[:3]
def check_offset_to_pinocchio_model_in_cart_space(self, q):
import pinocchio
from classic_framework.utils.geometric_transformation import mat2quat
obj_urdf = sim_framework_path("./envs/panda_arm_hand_pinocchio.urdf")
model = pinocchio.buildModelFromUrdf(obj_urdf)
data = model.createData()
q_pin = np.zeros(9)
q_pin[:7] = q
pinocchio.framesForwardKinematics(model, data, q_pin)
pinocchio.computeJointJacobians(model, data, q_pin)
pinocchio.framesForwardKinematics(model, data, q_pin)
# get orientation
rotation_matrix = data.oMf[model.getFrameId('panda_grasptarget')].rotation
quaternions = mat2quat(rotation_matrix) # [w x y z]
jac = pinocchio.getFrameJacobian(model, data, model.getFrameId('panda_grasptarget'),
pinocchio.LOCAL_WORLD_ALIGNED)[:, :7]
print('position offset:',
self.current_c_pos - np.array(data.oMf[model.getFrameId('panda_grasptarget')].translation))
def forward_kinematics(self, joint_positions):
"""
Compute end effector positions for the given joint configuration.
Args:
finger (SimFinger): a SimFinger object
joint_positions (list): Flat list of angular joint positions.
Returns:
List of end-effector positions. Each position is given as an
np.array with x,y,z positions.
"""
pinocchio.framesForwardKinematics(
self.robot_model, self.data, joint_positions,
)
return [
np.asarray(self.data.oMf[link_id].translation).reshape(-1).tolist()
for link_id in self.tip_link_ids
]
def inverse_kinematics(self, fid, xdes, q0):
"""
Method not in use right now, but is here with the intention
of using pinocchio for inverse kinematics instead of using
the in-house IK solver of pybullet.
"""
raise NotImplementedError()
dt = 1.0e-3
pinocchio.computeJointJacobians(
self.robot_model,
self.data,
q0,
)
pinocchio.framesKinematics(
self.robot_model,
self.data,
q0,
)
pinocchio.framesForwardKinematics(
self.robot_model,
self.data,
q0,
)
Ji = pinocchio.getFrameJacobian(
self.robot_model,
self.data,
fid,
pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED,
)[:3, :]
xcurrent = self.data.oMf[fid].translation
try:
Jinv = np.linalg.inv(Ji)
except Exception:
Jinv = np.linalg.pinv(Ji)
dq = Jinv.dot(xdes - xcurrent)
qnext = pinocchio.integrate(self.robot_model, q0, dt * dq)
return qnext
def compute_freeflyer_state_from_fixed_body(robot, position, velocity=None, acceleration=None,
fixed_body_name=None, ground_profile=None,
use_theoretical_model=True):
"""
@brief Fill rootjoint data from articular data when a body is fixed parallel to world.
@details If 'fixed_body_name' is omitted, it will default to the contact point that ensures
no contact points are going through the ground and a single one is touching it.
@remark The hypothesis is that 'fixed_body_name' is fixed parallel to world frame.
So this method computes the position of freeflyer rootjoint in the fixed body frame.
@note This function modifies internal data.
@param robot The jiminy robot
@param[inout] position Must contain current articular data. The rootjoint data can
contain any value, it will be ignored and replaced.
The method fills in rootjoint data.
@param[inout] velocity See position
@param[inout] acceleration See position
@param fixed_body_name The name of the body frame that is considered fixed parallel to world frame
@param ground_profile The ground profile callback
"""
if not robot.has_freeflyer:
raise ValueError("The robot does not have a freeflyer.")
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
position[:6].fill(0.0)
position[6] = 1.0
if velocity is not None and acceleration is not None:
velocity[:6].fill(0.0)
acceleration[:6].fill(0.0)
pin.forwardKinematics(pnc_model, pnc_data, position, velocity, acceleration)
elif velocity is not None:
velocity[:6].fill(0.0)
pin.forwardKinematics(pnc_model, pnc_data, position, velocity)
else:
pin.forwardKinematics(pnc_model, pnc_data, position)
pin.framesForwardKinematics(pnc_model, pnc_data, position)
if fixed_body_name is None:
fixed_body_name = _compute_closest_contact_frame(
robot, ground_profile, use_theoretical_model)
ff_M_fixed_body = get_body_world_transform(
robot, fixed_body_name, use_theoretical_model, copy=False)
if ground_profile is not None:
ground_translation = np.zeros(3)
ground_translation[2], normal = ground_profile(ff_M_fixed_body.translation)
ground_rotation = pin.Quaternion.FromTwoVectors(np.array([0.0, 0.0, 1.0]), normal).matrix()
w_M_ground = pin.SE3(ground_rotation, ground_translation)
else:
w_M_ground = pin.SE3.Identity()
w_M_ff = w_M_ground.act(ff_M_fixed_body.inverse())
position[:7] = pin.se3ToXYZQUAT(w_M_ff)
if velocity is not None:
ff_v_fixed_body = get_body_world_velocity(
robot, fixed_body_name, use_theoretical_model)
base_link_velocity = - ff_v_fixed_body
velocity[:6] = base_link_velocity.vector
if acceleration is not None:
ff_a_fixedBody = get_body_world_acceleration(
robot, fixed_body_name, use_theoretical_model)
base_link_acceleration = - ff_a_fixedBody
acceleration[:6] = base_link_acceleration.vector
return fixed_body_name
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)
urdf_pkg_path = get_package_share_directory("robot_properties_manipulator")
urdf_path = os.path.join(urdf_pkg_path, "urdf", "finger.urdf")
model = pinocchio.buildModelFromUrdf(urdf_path)
data = model.createData()
tip_link_id = model.getFrameId("finger_tip_link")
robot = robot_fingers.Robot(
robot_interfaces.finger,
robot_fingers.create_real_finger_backend,
"finger.yml",
)
robot.initialize()
action = robot.Action()
while True:
t = robot.frontend.append_desired_action(action)
joint_positions = robot.frontend.get_observation(t).position
# compute the forward kinematics
pinocchio.framesForwardKinematics(model, data, joint_positions)
# get the position of the tip link
pos = data.oMf[tip_link_id].translation
# convert from np.matrix to a flat array (for easy printing)
pos = np.asarray(pos).reshape(-1)
n_joints = len(pos)
format_string = "\r" + ", ".join(["{: 6.3f}"] * n_joints)
print(format_string.format(*pos), end="")
# --- Test vs CoM
print()
print("--- Test vs CoM ---")
estimator = SimpleZmpEstimator("ciao2")
model = pin.buildSampleModelHumanoid()
data = model.createData()
rightId = model.getFrameId('rleg_effector_body')
leftId = model.getFrameId('lleg_effector_body')
q = pin.neutral(model)
q[:3] = np.matrix([1.0, 0.0, 0.0]).T # displace freeflyer along x for testing
q[3:7] = np.matrix([np.sqrt(2) / 2, 0.0, np.sqrt(2) / 2, 0.0]).T # orient the base so that the feet are flat
pin.framesForwardKinematics(model, data, q)
poseRight = data.oMf[rightId].homogeneous.tolist()
poseLeft = data.oMf[leftId].homogeneous.tolist()
com = pin.centerOfMass(model, data, q).flatten().tolist()[0]
m = sum([inertia.mass for inertia in model.inertias[1:]])
g = 9.81
fz = m * g / 2.0
forceLeft = [0.0, 0.0, fz]
forceRight = [0.0, 0.0, fz]
lever = com[0] - data.oMf[rightId].translation[0]
tauy = -fz * lever
wrenchLeft = forceLeft + [0.0, tauy, 0.0]
wrenchRight = forceRight + [0.0, tauy, 0.0]
# Getting the frame index of each foot
ID_FL = solo.model.getFrameId("FL_FOOT")
ID_FR = solo.model.getFrameId("FR_FOOT")
ID_HL = solo.model.getFrameId("HL_FOOT")
ID_HR = solo.model.getFrameId("HR_FOOT")
dt = 0.01 #Integration step
#q[8]=np.pi/2
#q[7]=0
hist_err = [] #To see the error convergence
for i in range(2000):
pin.forwardKinematics(solo.model, solo.data, q)
pin.framesForwardKinematics(solo.model, solo.data, q) #useless ?
pin.updateFramePlacements(solo.model, solo.data)
# Getting the current height (on axis z) of each foot
hFL = solo.data.oMf[ID_FL].translation[2]
hFR = solo.data.oMf[ID_FR].translation[2]
hHL = solo.data.oMf[ID_HL].translation[2]
hHR = solo.data.oMf[ID_HR].translation[2]
# Computing the error in the world frame
err_FL = np.concatenate((np.zeros([2, 1]), hFL))
err_FR = np.concatenate((np.zeros([2, 1]), hFR))
err_HL = np.concatenate((np.zeros([2, 1]), hHL))
err_HR = np.concatenate((np.zeros([2, 1]), hHR))
# Computing the error in the local frame
def update(self, solo, qmes12, vmes12):
"""Update the quantities of the Interface based on the last measurements from the simulation
Args:
solo (object): Pinocchio wrapper for the quadruped
qmes12 (19x1 array): the position/orientation of the trunk and angular position of actuators
vmes12 (18x1 array): the linear/angular velocity of the trunk and angular velocity of actuators
"""
# Rotation matrix from the world frame to the base frame
self.oRb = pin.Quaternion(qmes12[3:7]).matrix()
# Linear and angular velocity in base frame
self.vmes12_base = vmes12.copy()
self.vmes12_base[0:3,
0:1] = self.oRb.transpose() @ self.vmes12_base[0:3,
0:1]
self.vmes12_base[3:6,
0:1] = self.oRb.transpose() @ self.vmes12_base[3:6,
0:1]
"""# Update Kinematics (required or automatically done by other functions?)
pin.forwardKinematics(solo.model, solo.data, qmes12, self.vmes12_base)
self.mean_feet_z = solo.data.oMf[self.indexes[0]].translation[2, 0]
for i in self.indexes[1:]:
self.mean_feet_z = np.min((self.mean_feet_z, solo.data.oMf[i].translation[2, 0]))
qmes12[2, 0] -= self.mean_feet_z"""
# Update Kinematics (required or automatically done by other functions?)
pin.forwardKinematics(solo.model, solo.data, qmes12, self.vmes12_base)
pin.framesForwardKinematics(solo.model, solo.data, qmes12)
# Get center of mass from Pinocchio
pin.centerOfMass(solo.model, solo.data, qmes12, self.vmes12_base)
# Update position/orientation of frames
pin.updateFramePlacements(solo.model, solo.data)
pin.ccrba(solo.model, solo.data, qmes12, self.vmes12_base)
# Update minimum height of feet
# TODO: Rename mean_feet_z into min_feet_z
self.mean_feet_z = solo.data.oMf[self.indexes[0]].translation[2, 0]
"""for i in self.indexes:
self.mean_feet_z += solo.data.oMf[i].translation[2, 0]
self.mean_feet_z *= 0.25"""
for i in self.indexes[1:]:
self.mean_feet_z = np.min(
(self.mean_feet_z, solo.data.oMf[i].translation[2, 0]))
self.mean_feet_z = 0.0
# Store position, linear velocity and angular velocity in global frame
self.oC = solo.data.com[0]
self.oV = solo.data.vcom[0]
self.oW = vmes12[3:6]
pin.crba(solo.model, solo.data, qmes12)
# Get SE3 object from world frame to base frame
self.oMb = pin.SE3(pin.Quaternion(qmes12[3:7]), self.oC)
self.RPY = pin.rpy.matrixToRpy(self.oMb.rotation)
# Get SE3 object from world frame to local frame
self.oMl = pin.SE3(
pin.utils.rotate('z', self.RPY[2, 0]),
np.array([qmes12[0, 0], qmes12[1, 0], self.mean_feet_z]))
# Get position, linear velocity and angular velocity in local frame
self.lC = self.oMl.inverse() * self.oC
self.lV = self.oMl.rotation.transpose() @ self.oV
self.lW = self.oMl.rotation.transpose() @ self.oW
# Pos, vel and acc of feet
for i, j in enumerate(self.indexes):
# Position of feet in local frame
self.o_feet[:, i:(i + 1)] = solo.data.oMf[j].translation
self.l_feet[:, i:(
i + 1)] = self.oMl.inverse() * solo.data.oMf[j].translation
# getFrameVelocity output is in the frame of the foot so a transform is required
self.ov_feet[:, i:(
i + 1)] = solo.data.oMf[j].rotation @ pin.getFrameVelocity(
solo.model, solo.data, j).vector[0:3, 0:1]
self.lv_feet[:, i:(
i + 1
)] = self.oMl.rotation.transpose() @ self.ov_feet[:, i:(i + 1)]
# getFrameAcceleration output is in the frame of the foot so a transform is required
self.oa_feet[:, i:(
i + 1)] = solo.data.oMf[j].rotation @ pin.getFrameAcceleration(
solo.model, solo.data, j).vector[0:3, 0:1]
self.la_feet[:, i:(
i + 1
)] = self.oMl.rotation.transpose() @ self.oa_feet[:, i:(i + 1)]
# Orientation of the base in local frame
# Base and local frames have the same yaw orientation in world frame
self.abg[0:2] = self.RPY[0:2]
# Position of shoulders in world frame
for i in range(4):
self.o_shoulders[:, i:(i + 1)] = self.oMl * self.l_shoulders[:, i]
return 0
e2q = EulerToQuat('e2q')
plug(base_estimator.q, e2q.euler)
e2q.quaternion.recompute(0)
print(e2q.quaternion.value)
print(len(e2q.quaternion.value))
q_est = np.matrix(e2q.quaternion.value).T
# --- Raw q difference ---
print("--- Raw q difference ---")
q_diff = q_est - q
print(q_diff.flatten().tolist()[0])
# --- Estimated feet ---
print("--- Estimated feet ---")
data2 = model.createData()
pin.framesForwardKinematics(model, data2, q_est)
print(data2.oMf[rightId])
print(data2.oMf[leftId])
# --- DCM estimator ---
print("--- DCM estimator ---")
dcm_estimator = DcmEstimator('dcm_estimator')
dcm_estimator.init(dt, robot_name)
plug(e2q.quaternion, dcm_estimator.q)
plug(base_estimator.v, dcm_estimator.v)
dcm_estimator.c.recompute(0)
print(dcm_estimator.c.value)
# --- Direct CoM ---
print("--- Direct CoM ---")
def residual(self, q):
pin.framesForwardKinematics(self.rmodel, self.rdata, q)
M = self.rdata.oMf[self.frameIndex]
return M.translation - self.ptarget
def updateGeometryPlacements(self, q):
pinocchio.framesForwardKinematics(self.model, self.data, np.array(q))
pinocchio.updateGeometryPlacements(self.model, self.data, self.gmodel,
self.gdata)
pio.Quaternion(-0.4, 0.02, -0.5, 0.7).normalized().matrix(),
np.matrix([.2, -.4, .7]).T)
gv.addXYZaxis('world/framegoal', [1., 0., 0., 1.], .015, .2)
gv.applyConfiguration('world/framegoal', list(pio.se3ToXYZQUAT(oMgoal).flat))
DT = 1e-2 # Integration step.
q0 = np.matrix([[
0., 0., 0., 1., 0.18, 1.37, -0.24, -0.98, 0.98, 0., 0., 0., 0., -0.13, 0.,
0., 0., 0.
]]).T
q = q0.copy()
herr = [] # Log the value of the error between tool and goal.
# Loop on an inverse kinematics for 200 iterations.
for i in range(200): # Integrate over 2 second of robot life
pio.framesForwardKinematics(robot.model, robot.data,
q) # Compute frame placements
oMtool = robot.data.oMf[
IDX_TOOL] # Placement from world frame o to frame f oMtool
oRtool = oMtool.rotation # Rotation from world axes to tool axes oRtool
tool_Jtool = pio.computeFrameJacobian(
robot.model, robot.data, q, IDX_TOOL) # 6D jacobian in local frame
o_Jtool3 = oRtool * tool_Jtool[:3, :] # 3D jacobian in world frame
o_TG = oMtool.translation - oMgoal.translation # vector from tool to goal, in world frame
vq = -pinv(o_Jtool3) * o_TG
q = pio.integrate(robot.model, q, vq * DT)
robot.display(q)
time.sleep(1e-3)
herr.append(o_TG)
def _apply_fk(self, q):
pin.framesForwardKinematics(self.wrapper.model, self.wrapper.data, q)
def get_oMf(self, q):
pin.framesForwardKinematics(self.wrapper.model, self.dummy_data, q)
return self.dummy_data.oMf
def compute_forward_kinematics(self, q):
'''
Computes forward kinematics of all the frames and stores in data
'''
pin.framesForwardKinematics(self.pin_robot.model, self.pin_robot.data,
q)
