4 from example_robot_data
import loadTalos
6 from pinocchio
import GeometryType
9 from os.path import join, dirname, abspath
15 state_name =
"half_sitting"
17 robot.q0 = robot.model.referenceConfigurations[state_name]
21 lfFoot, rfFoot, lhFoot, rhFoot = (
24 "gripper_left_fingertip_3_link",
25 "gripper_right_fingertip_3_link",
28 foot_frames = [lfFoot, rfFoot, lhFoot, rhFoot]
29 foot_frame_ids = [robot.model.getFrameId(frame_name)
for frame_name
in foot_frames]
31 robot.model.frames[robot.model.getFrameId(frame_name)].parent
32 for frame_name
in foot_frames
37 constraint_models = []
39 for j, frame_id
in enumerate(foot_frame_ids):
41 pinocchio.ContactType.CONTACT_6D,
43 robot.model.frames[frame_id].placement,
47 constraint_models.extend([contact_model_lf1])
52 np.array([0.6, -0.40, 1.0]),
60 robot.loadViewerModel(
"pinocchio")
61 gui = robot.viewer.gui
62 robot.display(robot.q0)
63 window_id = robot.viewer.gui.getWindowID(
"python-pinocchio")
65 robot.viewer.gui.setBackgroundColor1(window_id, [1.0, 1.0, 1.0, 1.0])
66 robot.viewer.gui.setBackgroundColor2(window_id, [1.0, 1.0, 1.0, 1.0])
67 robot.viewer.gui.addFloor(
"hpp-gui/floor")
69 robot.viewer.gui.setScale(
"hpp-gui/floor", [0.5, 0.5, 0.5])
70 robot.viewer.gui.setColor(
"hpp-gui/floor", [0.7, 0.7, 0.7, 1.0])
71 robot.viewer.gui.setLightingMode(
"hpp-gui/floor",
"OFF")
73 robot.display(robot.q0)
75 constraint_datas = [cm.createData()
for cm
in constraint_models]
81 constraint_dim = sum([cm.size()
for cm
in constraint_models])
96 y = np.ones((constraint_dim))
103 com_base = data.com[0].
copy()
106 com_des =
lambda k: com_base - np.array(
107 [0.0, 0.0, np.abs(com_drop_amp * np.sin(2.0 * np.pi * k * speed / (N_full)))]
113 com_act = data.com[0].
copy()
115 kkt_constraint.compute(model, data, constraint_models, constraint_datas, mu)
116 constraint_value = np.concatenate(
122 model, data, cm.joint1_id, cm.joint1_placement, cm.reference_frame
124 for cm
in constraint_models
127 primal_feas = np.linalg.norm(constraint_value, np.inf)
128 print(J.shape, constraint_value.shape, y.shape)
129 dual_feas = np.linalg.norm(J.T.dot(constraint_value + y), np.inf)
130 print(
"primal_feas:", primal_feas)
131 print(
"dual_feas:", dual_feas)
135 print(
"constraint_value:", np.linalg.norm(constraint_value))
136 print(
"com_error:", np.linalg.norm(com_err))
137 rhs = np.concatenate(
138 [-constraint_value - y * mu, kp * mass * com_err, np.zeros(model.nv - 3)]
140 dz = kkt_constraint.solve(rhs)
141 dy = dz[:constraint_dim]
142 dq = dz[constraint_dim:]
145 y -= alpha * (-dy + y)