5 from example_robot_data
import readParamsFromSrdf
6 from pinocchio
import JointModelFreeFlyer
8 model_dir =
"/local/rbudhira/devel/install/sot/share/example-robot-data/robots/"
10 sdf_filename = model_dir +
"cassie_description/robots/cassie.sdf"
11 srdf_path = model_dir +
"cassie_description/srdf/cassie_v2.srdf"
12 package_dir = model_dir +
"../../"
14 sdf_parent_guidance = [
19 "left-tarsus-spring-joint",
25 "right-tarsus-spring-joint",
30 robot = pinocchio.RobotWrapper.BuildFromSDF(
32 package_dirs=[package_dir],
34 root_link_name=
"pelvis",
35 parent_guidance=sdf_parent_guidance,
39 robot.model, srdf_path, has_rotor_parameters=
False, referencePose=
"standing"
43 constraint_models = robot.constraint_models
49 foot_joints = [
"left-plantar-foot-joint",
"right-plantar-foot-joint"]
50 foot_joint_ids = [robot.model.getJointId(joint_name)
for joint_name
in foot_joints]
52 front_placement =
pinocchio.SE3(np.identity(3), np.array([-0.1, 0.11, 0.0]))
54 back_placement =
pinocchio.SE3(np.identity(3), np.array([0.03, -0.0, 0.0]))
56 foot_length = np.linalg.norm(front_placement.translation - back_placement.translation)
61 mid_point = np.zeros(3)
63 for joint_id
in foot_joint_ids:
64 joint2_placement = robot.data.oMi[joint_id] * front_placement
65 joint2_placement.translation[2] = 0
67 pinocchio.ContactType.CONTACT_3D,
73 pinocchio.ReferenceFrame.LOCAL,
76 mid_point += joint2_placement.translation
77 joint2_placement.translation[0] -= foot_length
80 pinocchio.ContactType.CONTACT_3D,
86 pinocchio.ReferenceFrame.LOCAL,
89 mid_point += joint2_placement.translation
90 constraint_models.extend([contact_model_lf1, contact_model_lf2])
95 robot.initDisplay(loadModel=
True)
96 gui = robot.viewer.gui
98 robot.display(robot.q0)
100 constraint_datas = pinocchio.StdVec_RigidConstraintData()
101 for cm
in constraint_models:
102 constraint_datas.append(cm.createData())
107 for j, cm
in enumerate(robot.constraint_models):
108 pos1 = robot.data.oMi[cm.joint1_id] * cm.joint1_placement
109 pos2 = robot.data.oMi[cm.joint2_id] * cm.joint2_placement
110 name1 =
"hpp-gui/cm1_" + str(j)
111 name2 =
"hpp-gui/cm2_" + str(j)
112 gui.applyConfiguration(name1, list(pinocchio.SE3ToXYZQUAT(pos1)))
113 gui.applyConfiguration(name2, list(pinocchio.SE3ToXYZQUAT(pos2)))
118 for k
in range(model.joints[nj].nv):
119 for res
in range(200):
121 theta = res * 2.0 * np.pi / 200.0
122 v = np.zeros(robot.model.nv)
123 v[robot.model.idx_vs[nj] + k] = theta
134 constraint_dim = sum([cm.size()
for cm
in constraint_models])
141 window_id = robot.viewer.gui.getWindowID(
"python-pinocchio")
142 robot.viewer.gui.setBackgroundColor1(window_id, [1.0, 1.0, 1.0, 1.0])
143 robot.viewer.gui.setBackgroundColor2(window_id, [1.0, 1.0, 1.0, 1.0])
144 robot.viewer.gui.addFloor(
"hpp-gui/floor")
145 robot.viewer.gui.setScale(
"hpp-gui/floor", [0.5, 0.5, 0.5])
146 robot.viewer.gui.setColor(
"hpp-gui/floor", [0.7, 0.7, 0.7, 1.0])
147 robot.viewer.gui.setLightingMode(
"hpp-gui/floor",
"OFF")
154 for j, cm
in enumerate(constraint_models):
155 pos1 = robot.data.oMi[cm.joint1_id] * cm.joint1_placement
156 pos2 = robot.data.oMi[cm.joint2_id] * cm.joint2_placement
157 name1 =
"hpp-gui/cm1_" + str(j)
158 name2 =
"hpp-gui/cm2_" + str(j)
159 red_color = 1.0 * float(j) / float(len(constraint_models))
161 robot.viewer.gui.addXYZaxis(
162 name1, [red_color, 1.0, 1.0 - red_color, transparency], radius, axis_size
164 robot.viewer.gui.addXYZaxis(
165 name2, [red_color, 1.0, 1.0 - red_color, transparency], radius, axis_size
168 gui.applyConfiguration(name1, list(pinocchio.SE3ToXYZQUAT(pos1)))
169 gui.applyConfiguration(name2, list(pinocchio.SE3ToXYZQUAT(pos2)))
171 gui.setVisibility(name1,
"OFF")
172 gui.setVisibility(name2,
"OFF")
175 mid_point_name =
"hpp-gui/mid_point"
177 robot.viewer.gui.addXYZaxis(
178 mid_point_name, [0.0, 0.0, 1.0, transparency], radius, axis_size
182 gui.applyConfiguration(mid_point_name, list(pinocchio.SE3ToXYZQUAT(mid_point_pos)))
183 gui.setVisibility(mid_point_name,
"ALWAYS_ON_TOP")
189 mass = robot.data.mass[0]
191 com_base = robot.data.com[0].
copy()
192 com_base[:2] = mid_point[:2]
197 return com_base - np.array([0.0, 0.0, np.abs(com_drop_amp)])
200 def squashing(model, data, q_in, Nin=N, epsin=eps, verbose=True):
202 y = np.ones(constraint_dim)
208 kp = np.array([1.0, 1.0, 0.1])
213 com_act = robot.data.com[0].
copy()
215 kkt_constraint.compute(
216 robot.model, robot.data, constraint_models, constraint_datas, mu
218 constraint_value1 = np.concatenate(
219 [pinocchio.log(cd.c1Mc2)
for cd
in constraint_datas[:-4]]
221 constraint_value2 = np.concatenate(
222 [cd.c1Mc2.translation
for cd
in constraint_datas[-4:]]
224 constraint_value = np.concatenate([constraint_value1, constraint_value2])
235 for cm
in constraint_models[:-4]
247 for cm
in constraint_models[-4:]
250 J = np.vstack([J1, J2])
251 primal_feas = np.linalg.norm(constraint_value, np.inf)
253 dual_feas = np.linalg.norm(J.T.dot(constraint_value + y), np.inf)
254 if primal_feas < epsin
and dual_feas < epsin:
255 print(
"Convergence achieved")
258 print(
"constraint_value:", np.linalg.norm(constraint_value))
259 print(
"com_error:", np.linalg.norm(com_err))
261 print(
"com_act", com_act)
262 rhs = np.concatenate(
263 [-constraint_value - y * mu, kp * com_err, np.zeros(robot.model.nv - 3)]
265 dz = kkt_constraint.solve(rhs)
266 dy = dz[:constraint_dim]
267 dq = dz[constraint_dim:]
270 y -= alpha * (-dy + y)
273 gui.applyConfiguration(
281 qin = robot.q0.copy()
282 for k
in range(1000):
288 qout =
squashing(robot.model, data, qin, Nin=100000, epsin=1e-10, verbose=
False)