...
 
Commits (18)
Subproject commit 321eb1ccf1d94570eb564f3659b13ef3ef82239e
Subproject commit 72cf8cdefcf8cde818745ad7998122bde0b54734
<package format="2">
<name>sot-dynamic-pinocchio</name>
<version>3.4.5</version>
<version>3.5.1</version>
<description>
Pinocchio bindings for dynamic-graph
</description>
......@@ -18,14 +18,13 @@
<build_depend version_gte="1.1" version_lt="2.0">genmsg</build_depend>
<build_depend>sot-core</build_depend>
<build_depend>pinocchio</build_depend>
<build_depend>sot-tools</build_depend>
<build_depend>sot-core</build_depend>
<build_depend>pinocchio</build_depend>
<build_export_depend>pinocchio</build_export_depend>
<exec_depend>sot-tools</exec_depend>
<exec_depend>sot-core</exec_depend>
<exec_depend>pinocchio</exec_depend>
......
......@@ -49,7 +49,6 @@ IF(BUILD_PYTHON_INTERFACE)
__init__.py
humanoid_robot.py
tools.py
parser.py
)
FOREACH(py_file ${${PY_NAME}_PYTHON})
......
......@@ -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);
......
This diff is collapsed.
......@@ -133,7 +133,7 @@ def checkFinalConfiguration(position, finalPosition):
if 'argv' in sys.__dict__:
from optparse import OptionParser
from dynamic_graph.sot.dynamics.solver import Solver
from dynamic_graph.sot.dynamic_pinocchio.solver import Solver
# Parse options and enable robotviewer client if wanted.
clt = None
......
......@@ -42,3 +42,8 @@ FOREACH(test ${tests})
waist-attitude-from-sensor
)
ENDFOREACH(test)
IF(BUILD_PYTHON_INTERFACE)
ADD_SUBDIRECTORY(python)
ENDIF(BUILD_PYTHON_INTERFACE)
SET(${PROJECT_NAME}_PYTHON_TESTS
humanoid_robot
)
FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
ADD_PYTHON_UNIT_TEST("py-${TEST}" "tests/python/${TEST}.py")
ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
import os
from dynamic_graph.sot.dynamics.feet_follower import FeetFollowerFromFile
from dynamic_graph.sot.dynamics.tools import checkFinalConfiguration, clt, plug, robot, solver, timeStep
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')
......
......@@ -4,15 +4,44 @@
import unittest
from dynamic_graph.sot.dynamics.humanoid_robot import HumanoidRobot
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import AbstractHumanoidRobot
class HumanoidRobotTest(unittest.TestCase):
def test_simple(self):
class Robot(AbstractHumanoidRobot):
def __init__(self, name, urdfString=None, urdfFile=None):
if urdfString is not None:
self.loadModelFromString(urdfString)
elif urdfFile is not None:
self.loadModelFromUrdf(urdfFile)
else:
raise RuntimeError("You should provide either a URDF file or a URDF string")
AbstractHumanoidRobot.__init__(self, name, None)
def defineHalfSitting(self, q):
pass
def test_model_not_exist(self):
self.assertRaises(IOError, HumanoidRobot, "robot", True, "IDONOTEXIST")
class HumanoidRobotTest(unittest.TestCase):
def setUp(self):
import os
dir_path = os.path.dirname(os.path.realpath(__file__))
self.r2d2_urdf_file = os.path.join(dir_path, "r2d2.urdf")
def test_non_instanciable_robot(self):
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("test_build_robot_from_string", urdfString=urdfString)
def test_build_robot_from_urdf(self):
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>
......@@ -2,7 +2,7 @@
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
from dynamic_graph.sot.dynamics.tools import checkFinalConfiguration, clt, reach, robot, solver, timeStep
from dynamic_graph.sot.dynamic_pinocchio.tools import checkFinalConfiguration, clt, reach, robot, solver, timeStep
# Move left wrist
reach(robot, 'left-wrist', 0.25, 0.25, 0.5)
......
......@@ -2,7 +2,7 @@
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
from dynamic_graph.sot.dynamics.tools import checkFinalConfiguration, clt, reach, robot, solver, timeStep
from dynamic_graph.sot.dynamic_pinocchio.tools import checkFinalConfiguration, clt, reach, robot, solver, timeStep
# Move left wrist
reach(robot, 'left-wrist', 0.25, 0, 0.1)
......
......@@ -2,7 +2,7 @@
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
from dynamic_graph.sot.dynamics.tools import checkFinalConfiguration, clt, reach, robot, solver, timeStep
from dynamic_graph.sot.dynamic_pinocchio.tools import checkFinalConfiguration, clt, reach, robot, solver, timeStep
# Move right wrist
reach(robot, 'right-wrist', 0.25, 0, 0.1)
......