Commit 99345855 authored by Carlos Mastalli's avatar Carlos Mastalli
Browse files

[reduced] Added effort limits for reduced models

parent d9bd324d
......@@ -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:
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment