From 9934585566835f3e3cbbad7053aac516704bc07c Mon Sep 17 00:00:00 2001
From: Carlos Mastalli <carlos.mastalli@gmail.com>
Date: Sat, 14 Dec 2019 23:01:15 +0000
Subject: [PATCH] [reduced] Added effort limits for reduced models
---
python/example_robot_data/robots_loader.py | 30 +++++++++++++---------
1 file changed, 18 insertions(+), 12 deletions(-)
diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py
index 8d36a81..fa4b80b 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:
--
GitLab