...
 
Commits (6)
Subproject commit 321eb1ccf1d94570eb564f3659b13ef3ef82239e
Subproject commit a61ae61479a1d50d1ae3c988d1d9aaf20841173d
<package format="2">
<name>sot-dynamic-pinocchio</name>
<version>3.4.5</version>
<version>3.5.0</version>
<description>
Pinocchio bindings for dynamic-graph
</description>
......
......@@ -67,19 +67,18 @@ class GetJointNames : public Command {
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
GetJointNames(DynamicPinocchio& entity, const std::string& docstring)
: Command(entity, std::vector<Value::Type>(), docstring){}
: Command(entity, std::vector<Value::Type>(), docstring) {}
virtual Value doExecute() {
DynamicPinocchio& robot = static_cast<DynamicPinocchio&>(owner());
if (robot.m_model == 0x0){
SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
"model has not been initialized.");
if (robot.m_model == 0x0) {
SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC, "model has not been initialized.");
}
const std::vector<std::string>& jointNames = robot.m_model->names;
// Remove first joint names 'universe'
std::size_t n (jointNames.size());
std::size_t n(jointNames.size());
assert(n >= 1);
std::vector<Value> res;
for (std::size_t i=1; i<jointNames.size(); ++i) {
for (std::size_t i = 1; i < jointNames.size(); ++i) {
res.push_back(Value(jointNames[i]));
}
return Value(res);
......
......@@ -3,25 +3,25 @@
from __future__ import print_function
from functools import reduce
import sys
import pinocchio
from dynamic_graph import plug
from dynamic_graph.sot.core.derivator import Derivator_of_Vector
from dynamic_graph.sot.core.op_point_modifier import OpPointModifier
from dynamic_graph.sot.core.robot_simu import RobotSimu
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
from dynamic_graph.tools import addTrace
from dynamic_graph.tracer_real_time import TracerRealTime
import pinocchio, sys
if sys.version_info.major == 2:
from abc import ABCMeta, abstractmethod
class ABC:
__metaclass__ = ABCMeta
else:
from abc import ABC, abstractmethod
class AbstractRobot(ABC):
"""
This class instantiates all the entities required to get a consistent
......@@ -147,7 +147,7 @@ class AbstractRobot(ABC):
mimicJoints.append(name)
self.removeJoints(mimicJoints)
def removeJoints (self, joints):
def removeJoints(self, joints):
"""
- param joints: a list of joint names to be removed from the self.pinocchioModel
"""
......@@ -160,8 +160,7 @@ class AbstractRobot(ABC):
self.pinocchioModel = pinocchio.buildReducedModel(self.pinocchioModel, jointIds, q)
self.pinocchioData = pinocchio.Data(self.pinocchioModel)
def loadModelFromString (self, urdfString, rootJointType=pinocchio.JointModelFreeFlyer,
removeMimicJoints=True):
def loadModelFromString(self, urdfString, rootJointType=pinocchio.JointModelFreeFlyer, removeMimicJoints=True):
""" Load a URDF model contained in a string
- param rootJointType: the root joint type. None for no root joint.
- param removeMimicJoints: if True, all the mimic joints found in the model are removed.
......@@ -174,8 +173,11 @@ class AbstractRobot(ABC):
if removeMimicJoints:
self._removeMimicJoints(urdfString=urdfString)
def loadModelFromUrdf(self, urdfPath, urdfDir=None, rootJointType=pinocchio.JointModelFreeFlyer,
removeMimicJoints=True):
def loadModelFromUrdf(self,
urdfPath,
urdfDir=None,
rootJointType=pinocchio.JointModelFreeFlyer,
removeMimicJoints=True):
"""
Load a model using the pinocchio urdf parser. This parser looks
for urdf files in which kinematics and dynamics information
......@@ -185,10 +187,10 @@ class AbstractRobot(ABC):
"""
if urdfPath.startswith("package://"):
from os import path
n1 = 10 # len("package://")
n1 = 10 # len("package://")
n2 = urdfPath.index(path.sep, n1)
pkg = urdfPath[n1:n2]
relpath = urdfPath[n2+1:]
relpath = urdfPath[n2 + 1:]
import rospkg
rospack = rospkg.RosPack()
......@@ -233,7 +235,7 @@ class AbstractRobot(ABC):
q[joint.idx_q] = jv
@abstractmethod
def defineHalfSitting (self, q):
def defineHalfSitting(self, q):
"""
Define half sitting configuration using the pinocchio Model (i.e.
with quaternions and not with euler angles).
......@@ -256,7 +258,7 @@ class AbstractRobot(ABC):
raise RuntimeError("Dynamic robot model must be initialized first")
if not hasattr(self, 'device') or self.device is None:
#raise RuntimeError("A device is already defined.")
# raise RuntimeError("A device is already defined.")
self.device = RobotSimu(self.name + '_device')
self.device.resize(self.dynamic.getDimension())
"""
......@@ -271,13 +273,16 @@ class AbstractRobot(ABC):
"""
self.halfSitting = numpy.asarray(pinocchio.neutral(self.pinocchioModel)).flatten().tolist()
self.defineHalfSitting(self.halfSitting)
self.halfSitting[3:7] = [0., 0., 0.] # Replace quaternion by RPY.
self.halfSitting[3:7] = [0., 0., 0.] # Replace quaternion by RPY.
# Set the device limits.
def get(s):
s.recompute(0)
return s.value
def opposite(v): return [ -x for x in v ]
def opposite(v):
return [-x for x in v]
self.device.setPositionBounds(get(self.dynamic.lowerJl), get(self.dynamic.upperJl))
self.device.setVelocityBounds(opposite(get(self.dynamic.upperVl)), get(self.dynamic.upperVl))
self.device.setTorqueBounds(opposite(get(self.dynamic.upperTl)), get(self.dynamic.upperTl))
......@@ -394,6 +399,7 @@ 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)
class AbstractHumanoidRobot(AbstractRobot):
def __init__(self, name, tracer=None):
AbstractRobot.__init__(self, name, tracer)
......
......@@ -2,11 +2,12 @@
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
import os
print(os.environ)
from dynamic_graph.sot.dynamic_pinocchio.feet_follower import FeetFollowerFromFile
from dynamic_graph.sot.dynamic_pinocchio.tools import checkFinalConfiguration, clt, plug, robot, solver, timeStep
print(os.environ)
feetFollower = FeetFollowerFromFile('feet-follower')
feetFollower.feetToAnkleLeft = robot.dynamic.getAnklePositionInFootFrame()
......
......@@ -6,9 +6,9 @@ import unittest
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import AbstractHumanoidRobot
class Robot (AbstractHumanoidRobot):
def __init__ (self, name, urdfString=None, urdfFile=None):
import pinocchio
class Robot(AbstractHumanoidRobot):
def __init__(self, name, urdfString=None, urdfFile=None):
if urdfString is not None:
self.loadModelFromString(urdfString)
elif urdfFile is not None:
......@@ -18,7 +18,7 @@ class Robot (AbstractHumanoidRobot):
AbstractHumanoidRobot.__init__(self, name, None)
def defineHalfSitting (self, q):
def defineHalfSitting(self, q):
pass
......@@ -29,18 +29,19 @@ class HumanoidRobotTest(unittest.TestCase):
self.r2d2_urdf_file = os.path.join(dir_path, "r2d2.urdf")
def test_non_instanciable_robot(self):
class NonInstanciableRobot (AbstractHumanoidRobot):
class NonInstanciableRobot(AbstractHumanoidRobot):
pass
self.assertRaises(TypeError, NonInstanciableRobot, "non_instanciable_robot")
def test_build_robot_from_string(self):
with open(self.r2d2_urdf_file, 'r') as urdf:
urdfString = urdf.read()
robot = Robot("test_build_robot_from_string", urdfString = urdfString)
Robot("test_build_robot_from_string", urdfString=urdfString)
def test_build_robot_from_urdf(self):
robot = Robot("test_build_robot_from_string", urdfFile = self.r2d2_urdf_file)
Robot("test_build_robot_from_string", urdfFile=self.r2d2_urdf_file)
if __name__ == '__main__':
......
<!--
The robot model was taken from
https://wiki.ros.org/urdf/Tutorials/Building%20a%20Visual%20Robot%20Model%20with%20URDF%20from%20Scratch
-->
<robot name="test_build_robot_from_string">
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue"/>
</visual>
</link>
<link name="right_leg">
<visual>
<geometry>
<box size="0.6 0.1 0.2"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white"/>
</visual>
</link>
<joint name="base_to_right_leg" type="fixed">
<parent link="base_link"/>
<child link="right_leg"/>
<origin xyz="0 -0.22 0.25"/>
</joint>
<link name="left_leg">
<visual>
<geometry>
<box size="0.6 0.1 0.2"/>
</geometry>
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
<material name="white"/>
</visual>
</link>
<joint name="base_to_left_leg" type="fixed">
<parent link="base_link"/>
<child link="left_leg"/>
<origin xyz="0 0.22 0.25"/>
</joint>
</robot>