diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py index 8d36a81502d12923fee441f0ac9d593df46d74d6..fa4b80bcf270b833360bf6f9398b17dae049bb47 100644 --- a/python/example_robot_data/robots_loader.py +++ b/python/example_robot_data/robots_loader.py @@ -115,21 +115,27 @@ def loadTalosLegs(): for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias): if j.id < legMaxId: jid = m2.addJoint(parent, getattr(pinocchio, j.shortname())(), M, name) - up = m2.upperPositionLimit - down = m2.lowerPositionLimit - up[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq] - down[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq] - m2.upperPositionLimit = up - m2.lowerPositionLimit = down + upperPos = m2.upperPositionLimit + lowerPos = m2.lowerPositionLimit + effort = m2.effortLimit + upperPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq] + lowerPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq] + effort[m2.joints[jid].idx_v:m2.joints[jid].idx_v + j.nv] = m1.effortLimit[j.idx_v:j.idx_v + j.nv] + m2.upperPositionLimit = upperPos + m2.lowerPositionLimit = lowerPos + m2.effortLimit = effort assert (jid == j.id) m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity()) - u = m2.upperPositionLimit - u[:7] = 1 - m2.upperPositionLimit = u - limit = m2.lowerPositionLimit - limit[:7] = -1 - m2.lowerPositionLimit = limit + upperPos = m2.upperPositionLimit + upperPos[:7] = 1 + m2.upperPositionLimit = upperPos + lowerPos = m2.lowerPositionLimit + lowerPos[:7] = -1 + m2.lowerPositionLimit = lowerPos + effort = m2.effortLimit + effort[:6] = np.inf + m2.effortLimit = effort # q2 = robot.q0[:19] for f in m1.frames: