4 from example_robot_data
import load
6 from pinocchio
import GeometryType
9 from os.path import join, dirname, abspath
15 state_name =
"standing"
17 robot.q0 = robot.model.referenceConfigurations[state_name]
21 lfFoot, rfFoot, lhFoot, rhFoot =
"LF_FOOT",
"RF_FOOT",
"LH_FOOT",
"RH_FOOT"
23 foot_frames = [lfFoot, rfFoot, lhFoot, rhFoot]
24 foot_frame_ids = [robot.model.getFrameId(frame_name)
for frame_name
in foot_frames]
26 robot.model.frames[robot.model.getFrameId(frame_name)].parent
27 for frame_name
in foot_frames
32 constraint_models = []
34 for j, frame_id
in enumerate(foot_frame_ids):
36 pinocchio.ContactType.CONTACT_3D,
38 robot.model.frames[frame_id].placement,
43 constraint_models.extend([contact_model_lf1])
46 robot.loadViewerModel(
"pinocchio")
47 gui = robot.viewer.gui
48 robot.display(robot.q0)
49 window_id = robot.viewer.gui.getWindowID(
"python-pinocchio")
51 robot.viewer.gui.setBackgroundColor1(window_id, [1.0, 1.0, 1.0, 1.0])
52 robot.viewer.gui.setBackgroundColor2(window_id, [1.0, 1.0, 1.0, 1.0])
53 robot.viewer.gui.addFloor(
"hpp-gui/floor")
55 robot.viewer.gui.setScale(
"hpp-gui/floor", [0.5, 0.5, 0.5])
56 robot.viewer.gui.setColor(
"hpp-gui/floor", [0.7, 0.7, 0.7, 1.0])
57 robot.viewer.gui.setLightingMode(
"hpp-gui/floor",
"OFF")
59 robot.display(robot.q0)
61 constraint_datas = [cm.createData()
for cm
in constraint_models]
67 constraint_dim = sum([cm.size()
for cm
in constraint_models])
82 y = np.ones((constraint_dim))
89 com_base = data.com[0].
copy()
92 com_des =
lambda k: com_base - np.array(
93 [0.0, 0.0, np.abs(com_drop_amp * np.sin(2.0 * np.pi * k * speed / (N_full)))]
99 com_act = data.com[0].
copy()
101 kkt_constraint.compute(model, data, constraint_models, constraint_datas, mu)
102 constraint_value = np.concatenate(
103 [cd.c1Mc2.translation
for cd
in constraint_datas]
109 model, data, cm.joint1_id, cm.joint1_placement, cm.reference_frame
111 for cm
in constraint_models
114 primal_feas = np.linalg.norm(constraint_value, np.inf)
115 dual_feas = np.linalg.norm(J.T.dot(constraint_value + y), np.inf)
116 print(
"primal_feas:", primal_feas)
117 print(
"dual_feas:", dual_feas)
121 print(
"constraint_value:", np.linalg.norm(constraint_value))
122 print(
"com_error:", np.linalg.norm(com_err))
123 rhs = np.concatenate(
124 [-constraint_value - y * mu, kp * mass * com_err, np.zeros(model.nv - 3)]
126 dz = kkt_constraint.solve(rhs)
127 dy = dz[:constraint_dim]
128 dq = dz[constraint_dim:]
131 y -= alpha * (-dy + y)