Unverified Commit c6f9e372 authored by Carlos Mastalli's avatar Carlos Mastalli Committed by GitHub
Browse files

Bugfix: included the velocity limits in the Talos legs (#96)

* [bugfix] Included the velocity limits in the talos-legs model
parent fe981285
Pipeline #15024 passed with stage
in 1 minute and 47 seconds
......@@ -22,7 +22,7 @@ INCLUDE(cmake/python.cmake)
# Print initial message
MESSAGE(STATUS "${PROJECT_DESCRIPTION}, version ${PROJECT_VERSION}")
MESSAGE(STATUS "Copyright (C) 2018-2020 LAAS-CNRS / University of Edinburgh")
MESSAGE(STATUS "Copyright (C) 2018-2021 LAAS-CNRS, University of Edinburgh")
MESSAGE(STATUS "All rights reserved.")
MESSAGE(STATUS "Released under the BSD 3-Clause License.")
......
......@@ -162,17 +162,11 @@ class TalosLegsLoader(TalosLoader):
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(pin, j.shortname())(), M, name)
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
idx_q, idx_v = m2.joints[jid].idx_q, m2.joints[jid].idx_v
m2.upperPositionLimit[idx_q:idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq]
m2.lowerPositionLimit[idx_q:idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq]
m2.velocityLimit[idx_v:idx_v + j.nv] = m1.velocityLimit[j.idx_v:j.idx_v + j.nv]
m2.effortLimit[idx_v:idx_v + j.nv] = m1.effortLimit[j.idx_v:j.idx_v + j.nv]
assert jid == j.id
m2.appendBodyToJoint(jid, Y, pin.SE3.Identity())
......
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