4 from example_robot_data
import readParamsFromSrdf
5 from pinocchio
import buildModelFromSdf, buildGeomFromSdf, neutral, JointModelFreeFlyer
7 from pinocchio
import GeometryType
10 from os.path import join, dirname, abspath
13 model_dir =
"/local/rbudhira/devel/install/sot/share/example-robot-data/robots/"
15 sdf_filename = model_dir +
"cassie_description/robots/cassie.sdf"
16 srdf_path = model_dir +
"cassie_description/srdf/cassie_v2.srdf"
17 package_dir = model_dir +
"../../"
19 sdf_parent_guidance = [
24 "left-tarsus-spring-joint",
30 "right-tarsus-spring-joint",
35 robot = pinocchio.RobotWrapper.BuildFromSDF(
37 package_dirs=[package_dir],
39 root_link_name=
"pelvis",
40 parent_guidance=sdf_parent_guidance,
44 robot.model, srdf_path, has_rotor_parameters=
False, referencePose=
"standing"
48 constraint_models = robot.constraint_models
54 foot_joints = [
"left-plantar-foot-joint",
"right-plantar-foot-joint"]
55 foot_joint_ids = [robot.model.getJointId(joint_name)
for joint_name
in foot_joints]
57 front_placement =
pinocchio.SE3(np.identity(3), np.array([-0.1, 0.11, 0.0]))
59 back_placement =
pinocchio.SE3(np.identity(3), np.array([0.03, -0.0, 0.0]))
61 foot_length = np.linalg.norm(front_placement.translation - back_placement.translation)
66 mid_point = np.zeros(3)
68 for joint_id
in foot_joint_ids:
69 joint2_placement = robot.data.oMi[joint_id] * front_placement
70 joint2_placement.translation[2] = 0
72 pinocchio.ContactType.CONTACT_3D,
78 pinocchio.ReferenceFrame.LOCAL,
81 mid_point += joint2_placement.translation
82 joint2_placement.translation[0] -= foot_length
85 pinocchio.ContactType.CONTACT_3D,
91 pinocchio.ReferenceFrame.LOCAL,
94 mid_point += joint2_placement.translation
95 constraint_models.extend([contact_model_lf1, contact_model_lf2])
100 robot.initDisplay(loadModel=
True)
101 gui = robot.viewer.gui
103 robot.display(robot.q0)
105 constraint_datas = pinocchio.StdVec_RigidConstraintData()
106 for cm
in constraint_models:
107 constraint_datas.append(cm.createData())
112 for j, cm
in enumerate(robot.constraint_models):
113 pos1 = robot.data.oMi[cm.joint1_id] * cm.joint1_placement
114 pos2 = robot.data.oMi[cm.joint2_id] * cm.joint2_placement
115 name1 =
"hpp-gui/cm1_" + str(j)
116 name2 =
"hpp-gui/cm2_" + str(j)
117 gui.applyConfiguration(name1, list(pinocchio.SE3ToXYZQUAT(pos1)))
118 gui.applyConfiguration(name2, list(pinocchio.SE3ToXYZQUAT(pos2)))
123 for k
in range(model.joints[nj].nv):
124 for res
in range(200):
126 theta = res * 2.0 * np.pi / 200.0
127 v = np.zeros(robot.model.nv)
128 v[robot.model.idx_vs[nj] + k] = theta
139 constraint_dim = sum([cm.size()
for cm
in constraint_models])
146 window_id = robot.viewer.gui.getWindowID(
"python-pinocchio")
147 robot.viewer.gui.setBackgroundColor1(window_id, [1.0, 1.0, 1.0, 1.0])
148 robot.viewer.gui.setBackgroundColor2(window_id, [1.0, 1.0, 1.0, 1.0])
149 robot.viewer.gui.addFloor(
"hpp-gui/floor")
150 robot.viewer.gui.setScale(
"hpp-gui/floor", [0.5, 0.5, 0.5])
151 robot.viewer.gui.setColor(
"hpp-gui/floor", [0.7, 0.7, 0.7, 1.0])
152 robot.viewer.gui.setLightingMode(
"hpp-gui/floor",
"OFF")
159 for j, cm
in enumerate(constraint_models):
160 pos1 = robot.data.oMi[cm.joint1_id] * cm.joint1_placement
161 pos2 = robot.data.oMi[cm.joint2_id] * cm.joint2_placement
162 name1 =
"hpp-gui/cm1_" + str(j)
163 name2 =
"hpp-gui/cm2_" + str(j)
164 red_color = 1.0 * float(j) / float(len(constraint_models))
166 robot.viewer.gui.addXYZaxis(
167 name1, [red_color, 1.0, 1.0 - red_color, transparency], radius, axis_size
169 robot.viewer.gui.addXYZaxis(
170 name2, [red_color, 1.0, 1.0 - red_color, transparency], radius, axis_size
173 gui.applyConfiguration(name1, list(pinocchio.SE3ToXYZQUAT(pos1)))
174 gui.applyConfiguration(name2, list(pinocchio.SE3ToXYZQUAT(pos2)))
176 gui.setVisibility(name1,
"OFF")
177 gui.setVisibility(name2,
"OFF")
180 mid_point_name =
"hpp-gui/mid_point"
182 robot.viewer.gui.addXYZaxis(
183 mid_point_name, [0.0, 0.0, 1.0, transparency], radius, axis_size
187 gui.applyConfiguration(mid_point_name, list(pinocchio.SE3ToXYZQUAT(mid_point_pos)))
188 gui.setVisibility(mid_point_name,
"ALWAYS_ON_TOP")
194 mass = robot.data.mass[0]
196 com_base = robot.data.com[0].
copy()
197 com_base[:2] = mid_point[:2]
199 com_des =
lambda k: com_base - np.array([0.0, 0.0, np.abs(com_drop_amp)])
202 def squashing(model, data, q_in, Nin=N, epsin=eps, verbose=True):
204 y = np.ones((constraint_dim))
210 kp = np.array([1.0, 1.0, 0.1])
215 com_act = robot.data.com[0].
copy()
217 kkt_constraint.compute(
218 robot.model, robot.data, constraint_models, constraint_datas, mu
220 constraint_value1 = np.concatenate(
221 [pinocchio.log(cd.c1Mc2)
for cd
in constraint_datas[:-4]]
223 constraint_value2 = np.concatenate(
224 [cd.c1Mc2.translation
for cd
in constraint_datas[-4:]]
226 constraint_value = np.concatenate([constraint_value1, constraint_value2])
237 for cm
in constraint_models[:-4]
249 for cm
in constraint_models[-4:]
252 J = np.vstack([J1, J2])
253 primal_feas = np.linalg.norm(constraint_value, np.inf)
255 dual_feas = np.linalg.norm(J.T.dot(constraint_value + y), np.inf)
256 if primal_feas < epsin
and dual_feas < epsin:
257 print(
"Convergence achieved")
260 print(
"constraint_value:", np.linalg.norm(constraint_value))
261 print(
"com_error:", np.linalg.norm(com_err))
263 print(
"com_act", com_act)
264 rhs = np.concatenate(
265 [-constraint_value - y * mu, kp * com_err, np.zeros(robot.model.nv - 3)]
267 dz = kkt_constraint.solve(rhs)
268 dy = dz[:constraint_dim]
269 dq = dz[constraint_dim:]
272 y -= alpha * (-dy + y)
275 gui.applyConfiguration(
283 qin = robot.q0.copy()
284 for k
in range(1000):
290 qout =
squashing(robot.model, data, qin, Nin=100000, epsin=1e-10, verbose=
False)