Unverified Commit f35ef164 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1009 from jcarpent/devel

Rename function in Python for uniformization of namings
parents 4cbed01b 5f4a7ff2
......@@ -92,3 +92,15 @@ class GeometryObject(pin.GeometryObject):
def fcl(self):
return self.geometry
@deprecated("This function is now called SE3ToXYZQUATtuple. Please change for this new signature to delete this warning.")
def se3ToXYZQUATtuple(M):
return pin.SE3ToXYZQUATtuple(M)
@deprecated("This function is now called SE3ToXYZQUAT. Please change for this new signature to delete this warning.")
def se3ToXYZQUAT(M):
return pin.SE3ToXYZQUAT(M)
@deprecated("This function is now called XYZQUATToSE3. Please change for this new signature to delete this warning.")
def XYZQUATToSe3(x):
return pin.XYZQUATToSE3(x)
#
# Copyright (c) 2015-2019 CNRS INRIA
# Copyright (c) 2015-2020 CNRS INRIA
#
from __future__ import print_function
......@@ -34,29 +34,7 @@ def se3ToXYZQUAT(M):
@deprecated('Now useless. You can directly have access to this function from the main scope of Pinocchio')
def XYZQUATToSe3(vec):
return pin.XYZQUATToSe3(vec)
@deprecated('Now useless.')
def XYZQUATToViewerConfiguration(xyzq):
'''
Convert the input 7D vector [X,Y,Z,x,y,z,w] to 7D vector [X,Y,Z,x,y,z,w]
Gepetto Viewer Corba has changed its convention for quaternions - This function is not more required.
See https://github.com/humanoid-path-planner/gepetto-viewer-corba/pull/58 for more details.
'''
if isinstance(xyzq, (np.matrix)):
return xyzq.A.squeeze().tolist()
return xyzq
@deprecated('Now useless.')
def ViewerConfigurationToXYZQUAT(vconf):
'''
Reverse function of XYZQUATToViewerConfiguration : convert [X,Y,Z,x,y,z,w] to [X,Y,Z,x,y,z,w]
Gepetto Viewer Corba has changed its convention for quaternions - This function is not more required.
See https://github.com/humanoid-path-planner/gepetto-viewer-corba/pull/58 for more details.
'''
return vconf
return pin.XYZQUATToSE3(vec)
def isapprox(a, b, epsilon=1e-6):
if "np" in a.__class__.__dict__:
......@@ -112,4 +90,4 @@ __all__ = ['np', 'npl', 'eye', 'zero', 'rand', 'isapprox', 'mprint',
'npToTTuple', 'npToTuple', 'rotate',
'rpyToMatrix', 'matrixToRpy',
'se3ToXYZQUAT', 'XYZQUATToSe3',
'XYZQUATToViewerConfiguration', 'ViewerConfigurationToXYZQUAT', 'fromListToVectorOfString']
'fromListToVectorOfString']
......@@ -148,14 +148,14 @@ class GepettoVisualizer(BaseVisualizer):
pin.updateGeometryPlacements(self.model, self.data, self.collision_model, self.collision_data)
gui.applyConfigurations (
[ self.getViewerNodeName(collision,pin.GeometryType.COLLISION) for collision in self.collision_model.geometryObjects ],
[ pin.se3ToXYZQUATtuple(self.collision_data.oMg[self.collision_model.getGeometryId(collision.name)]) for collision in self.collision_model.geometryObjects ]
[ pin.SE3ToXYZQUATtuple(self.collision_data.oMg[self.collision_model.getGeometryId(collision.name)]) for collision in self.collision_model.geometryObjects ]
)
if self.display_visuals:
pin.updateGeometryPlacements(self.model, self.data, self.visual_model, self.visual_data)
gui.applyConfigurations (
[ self.getViewerNodeName(visual,pin.GeometryType.VISUAL) for visual in self.visual_model.geometryObjects ],
[ pin.se3ToXYZQUATtuple(self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]) for visual in self.visual_model.geometryObjects ]
[ pin.SE3ToXYZQUATtuple(self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]) for visual in self.visual_model.geometryObjects ]
)
gui.refresh()
......
//
// Copyright (c) 2019 CNRS
// Copyright (c) 2019-2020 CNRS INRIA
//
#include "pinocchio/bindings/python/fwd.hpp"
......@@ -12,11 +12,11 @@ namespace pinocchio
namespace bp = boost::python;
typedef SE3::Scalar Scalar;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic,1> VectorXd;
typedef Eigen::Matrix<Scalar, 7, 1> Vector7d;
typedef Eigen::Matrix<Scalar, 7,1> Vector7d;
typedef Eigen::Map< SE3::Quaternion> QuatMap;
typedef Eigen::Map<const SE3::Quaternion> QuatConstMap;
VectorXd se3ToXYZQUAT (const SE3& M)
VectorXd SE3ToXYZQUAT(const SE3& M)
{
Vector7d res;
res.head<3>() = M.translation();
......@@ -24,7 +24,7 @@ namespace pinocchio
return res;
}
bp::tuple se3ToXYZQUATtuple (const SE3& M)
bp::tuple SE3ToXYZQUATtuple(const SE3& M)
{
SE3::Quaternion q (M.rotation());
return bp::make_tuple (
......@@ -32,8 +32,8 @@ namespace pinocchio
q.x(), q.y(), q.z(), q.w());
}
template <typename TupleOfList>
SE3 XYZQUATToSe3_bp(const TupleOfList& v)
template <typename TupleOrList>
SE3 XYZQUATToSE3_bp(const TupleOrList& v)
{
//bp::extract<SE3::Scalar> to_double;
SE3::Quaternion q (
......@@ -49,7 +49,7 @@ namespace pinocchio
}
template <typename Vector7Like>
SE3 XYZQUATToSe3_ei(const Vector7Like& v)
SE3 XYZQUATToSE3_ei(const Vector7Like& v)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector7Like, v, 7, 1);
QuatConstMap q (v.template tail<4>().data());
......@@ -58,15 +58,20 @@ namespace pinocchio
void exposeConversions()
{
const char* doc1 = "Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,Q1,Q2,Q3,Q4] .";
bp::def ("se3ToXYZQUAT" , se3ToXYZQUAT , doc1);
bp::def ("se3ToXYZQUATtuple", se3ToXYZQUATtuple, doc1);
const char* doc1 = "Convert the input SE3 object to a numpy array.";
bp::def("SE3ToXYZQUAT" , SE3ToXYZQUAT , "M", doc1);
const char* doc1_tuple = "Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,x,y,z,w].";
bp::def("SE3ToXYZQUATtuple", SE3ToXYZQUATtuple, "M", doc1_tuple);
const char* doc2 = "Reverse function of se3ToXYZQUAT: convert [X,Y,Z,Q1,Q2,Q3,Q4] to a SE3 element";
bp::def ("XYZQUATToSe3", static_cast<SE3 (*) (const bp::tuple&)> (XYZQUATToSe3_bp<bp::tuple>), doc2);
bp::def ("XYZQUATToSe3", static_cast<SE3 (*) (const bp::list &)> (XYZQUATToSe3_bp<bp::list >), doc2);
bp::def ("XYZQUATToSe3", static_cast<SE3 (*) (const VectorXd &)> (XYZQUATToSe3_ei<VectorXd >), doc2);
bp::def ("XYZQUATToSe3", static_cast<SE3 (*) (const Vector7d &)> (XYZQUATToSe3_ei<Vector7d >), doc2);
const char* doc2 = "Reverse function of SE3ToXYZQUAT: convert [X,Y,Z,x,y,z,w] to an SE3 element.";
bp::def("XYZQUATToSE3",
static_cast<SE3 (*) (const bp::tuple&)> (XYZQUATToSE3_bp<bp::tuple>),
bp::arg("tuple"),doc2);
bp::def("XYZQUATToSE3",
static_cast<SE3 (*) (const bp::list &)> (XYZQUATToSE3_bp<bp::list >),
bp::arg("list"),doc2);
bp::def("XYZQUATToSE3", static_cast<SE3 (*) (const VectorXd &)> (XYZQUATToSE3_ei<VectorXd >),
bp::arg("array"),doc2);
}
} // namespace python
......
......@@ -35,16 +35,16 @@ viz = GepettoVisualizer(
# Initialize the viewer.
try:
viz.initViewer()
except ImportError:
except ImportError as error:
print("Error while initializing the viewer. It seems you should install gepetto-viewer")
print(err)
print(error.message)
sys.exit(0)
try:
viz.loadViewerModel("shapes")
except AttributeError:
except AttributeError as error:
print("Error while loading the viewer model. It seems you should start gepetto-viewer")
print(err)
print(error.message)
sys.exit(0)
viz.display(np.zeros(0))
......@@ -114,11 +114,11 @@ class TestSE3Bindings(unittest.TestCase):
def test_conversions(self):
def compute (m):
tq_vec = pin.se3ToXYZQUAT (m)
tq_tup = pin.se3ToXYZQUATtuple (m)
mm_vec = pin.XYZQUATToSe3 (tq_vec)
mm_tup = pin.XYZQUATToSe3 (tq_tup)
mm_lis = pin.XYZQUATToSe3 (list(tq_tup))
tq_vec = pin.SE3ToXYZQUAT (m)
tq_tup = pin.SE3ToXYZQUATtuple (m)
mm_vec = pin.XYZQUATToSE3 (tq_vec)
mm_tup = pin.XYZQUATToSE3 (tq_tup)
mm_lis = pin.XYZQUATToSE3 (list(tq_tup))
return tq_vec, tq_tup, mm_vec, mm_tup, mm_lis
m = pin.SE3.Identity()
......
......@@ -19,8 +19,8 @@ class TestUtils(TestCase):
m = pin.SE3.Identity()
m.translation = np.matrix('1. 2. 3.').T
m.rotation = np.matrix('1. 0. 0.;0. 0. -1.;0. 1. 0.') # rotate('x', pi / 2)
self.assertApprox(pin.se3ToXYZQUAT(m).T, [1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2])
self.assertApprox(pin.XYZQUATToSe3([1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m)
self.assertApprox(pin.SE3ToXYZQUAT(m).T, [1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2])
self.assertApprox(pin.XYZQUATToSE3([1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m)
def test_isapprox(self):
self.assertFalse(isapprox(1, 2))
......
Supports Markdown
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