Merge branch 'topic/talos-limits' into 'devel'

[reduced] Added effort limits for reduced models

See merge request !31

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: