Skip to content
Snippets Groups Projects
Unverified Commit d03438b5 authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #90 from florent-lamiraux/devel

[python] Add method to get actuated joints
parents aa1a11ce b191d478
No related branches found
No related tags found
No related merge requests found
......@@ -147,6 +147,16 @@ class AbstractRobot(ABC):
mimicJoints.append(name)
self.removeJoints(mimicJoints)
def _storeRootJointType(self, rootJointType):
if rootJointType == pinocchio.JointModelFreeFlyer:
self.rootJointType = 'freeflyer'
elif rootJointType == pinocchio.JointModelPlanar:
self.rootJointType = 'planar'
elif rootJointType is None:
self.rootJointType = 'fixed'
else:
raise TypeError('rootJointType should be either JointModelFreeflyer, JointModelPlanar, or None.')
def removeJoints(self, joints):
"""
- param joints: a list of joint names to be removed from the self.pinocchioModel
......@@ -172,6 +182,7 @@ class AbstractRobot(ABC):
self.pinocchioData = pinocchio.Data(self.pinocchioModel)
if removeMimicJoints:
self._removeMimicJoints(urdfString=urdfString)
self._storeRootJointType(rootJointType)
def loadModelFromUrdf(self,
urdfPath,
......@@ -208,6 +219,7 @@ class AbstractRobot(ABC):
self.pinocchioData = pinocchio.Data(self.pinocchioModel)
if removeMimicJoints:
self._removeMimicJoints(urdfFile=urdfFile)
self._storeRootJointType(rootJointType)
def initializeOpPoints(self):
for op in self.OperationalPoints:
......@@ -406,6 +418,22 @@ class AbstractRobot(ABC):
self.dynamic.signal(self.OperationalPointsMap[op]).recompute(self.device.state.time + 1)
self.dynamic.signal('J' + self.OperationalPointsMap[op]).recompute(self.device.state.time + 1)
def getActuatedJoints(self):
"""
Return the list of indices in the velocity vector of actuated
joints.
This method is particularly useful to define a posture task where only
the actuated joints are controlled.
"""
if self.rootJointType == "freeflyer":
return range(6, self.dynamic.model.nv)
if self.rootJointType == "planar":
return range(3, self.dynamic.model.nv)
if self.rootJointType == "fixed":
return range(0, self.dynamic.model.nv)
raise TypeError("unknown rootJointType")
class AbstractHumanoidRobot(AbstractRobot):
def __init__(self, name, tracer=None):
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment