anymal-simulation.py
Go to the documentation of this file.
1 from time import sleep
2 
3 import numpy as np
4 import pinocchio
5 from example_robot_data import load
6 
7 robot = load("anymal")
8 model = robot.model
9 data = robot.data
10 
11 state_name = "standing"
12 
13 robot.q0 = robot.model.referenceConfigurations[state_name]
14 
15 pinocchio.forwardKinematics(model, data, robot.q0)
16 
17 lfFoot, rfFoot, lhFoot, rhFoot = "LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"
18 
19 foot_frames = [lfFoot, rfFoot, lhFoot, rhFoot]
20 foot_frame_ids = [robot.model.getFrameId(frame_name) for frame_name in foot_frames]
21 foot_joint_ids = [
22  robot.model.frames[robot.model.getFrameId(frame_name)].parent
23  for frame_name in foot_frames
24 ]
25 pinocchio.forwardKinematics(model, data, robot.q0)
26 pinocchio.framesForwardKinematics(model, data, robot.q0)
27 
28 constraint_models = []
29 
30 for j, frame_id in enumerate(foot_frame_ids):
31  contact_model_lf1 = pinocchio.RigidConstraintModel(
32  pinocchio.ContactType.CONTACT_3D,
33  robot.model,
34  foot_joint_ids[j],
35  robot.model.frames[frame_id].placement,
36  0,
37  data.oMf[frame_id],
38  )
39 
40  constraint_models.extend([contact_model_lf1])
41 
42 robot.initViewer()
43 robot.loadViewerModel("pinocchio")
44 robot.display(robot.q0)
45 
46 constraint_datas = [cm.createData() for cm in constraint_models]
47 
48 q = robot.q0.copy()
49 
50 pinocchio.computeAllTerms(model, data, q, np.zeros(model.nv))
51 kkt_constraint = pinocchio.ContactCholeskyDecomposition(model, constraint_models)
52 constraint_dim = sum([cm.size() for cm in constraint_models])
53 N = 100000
54 eps = 1e-10
55 mu = 0.0
56 # q_sol = (q[:] + np.pi) % np.pi - np.pi
57 q_sol = q.copy()
58 robot.display(q_sol)
59 
60 # Bring CoM between the two feet.
61 
62 mass = data.mass[0]
63 
64 
65 def squashing(model, data, q_in):
66  q = q_in.copy()
67  y = np.ones(constraint_dim)
68 
69  N_full = 200
70 
71  # Decrease CoMz by 0.2
72  com_drop_amp = 0.2
73  pinocchio.computeAllTerms(model, data, q, np.zeros(model.nv))
74  com_base = data.com[0].copy()
75  kp = 1.0
76  speed = 1.0
77 
78  def com_des(k):
79  return com_base - np.array(
80  [
81  0.0,
82  0.0,
83  np.abs(com_drop_amp * np.sin(2.0 * np.pi * k * speed / (N_full))),
84  ]
85  )
86 
87  for k in range(N):
88  pinocchio.computeAllTerms(model, data, q, np.zeros(model.nv))
89  pinocchio.computeJointJacobians(model, data, q)
90  pinocchio.computeJointJacobians(model, data, q)
91  com_act = data.com[0].copy()
92  com_err = com_act - com_des(k)
93  kkt_constraint.compute(model, data, constraint_models, constraint_datas, mu)
94  constraint_value = np.concatenate(
95  [cd.c1Mc2.translation for cd in constraint_datas]
96  )
97  # constraint_value = np.concatenate(
98  # [pinocchio.log6(cd.c1Mc2) for cd in constraint_datas]
99  # )
100  J = np.vstack(
101  [
103  model, data, cm.joint1_id, cm.joint1_placement, cm.reference_frame
104  )[:3, :]
105  for cm in constraint_models
106  ]
107  )
108  primal_feas = np.linalg.norm(constraint_value, np.inf)
109  dual_feas = np.linalg.norm(J.T.dot(constraint_value + y), np.inf)
110  print("primal_feas:", primal_feas)
111  print("dual_feas:", dual_feas)
112  # if primal_feas < eps and dual_feas < eps:
113  # print("Convergence achieved")
114  # break
115  print("constraint_value:", np.linalg.norm(constraint_value))
116  print("com_error:", np.linalg.norm(com_err))
117  rhs = np.concatenate(
118  [-constraint_value - y * mu, kp * mass * com_err, np.zeros(model.nv - 3)]
119  )
120  dz = kkt_constraint.solve(rhs)
121  dy = dz[:constraint_dim]
122  dq = dz[constraint_dim:]
123  alpha = 1.0
124  q = pinocchio.integrate(model, q, -alpha * dq)
125  y -= alpha * (-dy + y)
126  robot.display(q)
127  sleep(0.05)
128  return q
129 
130 
131 q_new = squashing(model, data, robot.q0)
cassie-simulation.com_des
def com_des(k)
Definition: cassie-simulation.py:164
boost::serialization::load
void load(Archive &ar, Eigen::array< _IndexType, _NumIndices > &a, const unsigned int)
Definition: serialization/eigen.hpp:241
pinocchio::forwardKinematics
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
Update the joint placements, spatial velocities and spatial accelerations according to the current jo...
pinocchio::RigidConstraintModelTpl
Definition: algorithm/constraints/fwd.hpp:14
anymal-simulation.squashing
def squashing(model, data, q_in)
Definition: anymal-simulation.py:65
pinocchio::integrate
void integrate(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout)
Visit a LieGroupVariant to call its integrate method.
pinocchio::framesForwardKinematics
void framesForwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
First calls the forwardKinematics on the model, then computes the placement of each frame....
pinocchio::computeJointJacobians
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame....
pinocchio::getFrameJacobian
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > getFrameJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame reference_frame)
Returns the jacobian of the frame expressed either expressed in the local frame coordinate system,...
Definition: frames.hpp:394
pinocchio::copy
void copy(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &origin, DataTpl< Scalar, Options, JointCollectionTpl > &dest, KinematicLevel kinematic_level)
Copy part of the data from origin to dest. Template parameter can be used to select at which differen...
Definition: copy.hpp:42
pinocchio::ContactCholeskyDecompositionTpl< context::Scalar, context::Options >
pinocchio::computeAllTerms
void computeAllTerms(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes efficiently all the terms needed for dynamic simulation. It is equivalent to the call at the...


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:06