Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Topic/check xyzquat conversion #2073

Merged
merged 2 commits into from
Nov 1, 2023
Merged

Topic/check xyzquat conversion #2073

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
fd1f2a4
python: Throw an exception in XYZQUATToSE3 if called with wrong size …
jorisv Oct 31, 2023
1201d37
python: apply review
jorisv Oct 31, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 21 additions & 9 deletions bindings/python/utils/conversions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,8 @@
// Copyright (c) 2019-2020 CNRS INRIA
//

#include <sstream>

#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/bindings/python/spatial/se3.hpp"

Expand Down Expand Up @@ -35,23 +37,33 @@ namespace pinocchio
template <typename TupleOrList>
SE3 XYZQUATToSE3_bp(const TupleOrList& v)
{
//bp::extract<SE3::Scalar> to_double;
ssize_t size = bp::len(v);
if(size < 7)
jorisv marked this conversation as resolved.
Show resolved Hide resolved
{
throw std::invalid_argument(
"Wrong size: v(" + std::to_string(size) + ") should at least have 7 elements");
}
SE3::Quaternion q (
(Scalar)bp::extract<Scalar>(v[6]),
(Scalar)bp::extract<Scalar>(v[3]),
(Scalar)bp::extract<Scalar>(v[4]),
(Scalar)bp::extract<Scalar>(v[5]));
static_cast<Scalar>(bp::extract<Scalar>(v[6])),
static_cast<Scalar>(bp::extract<Scalar>(v[3])),
static_cast<Scalar>(bp::extract<Scalar>(v[4])),
static_cast<Scalar>(bp::extract<Scalar>(v[5])));
SE3::Vector3 t (
(Scalar)bp::extract<Scalar>(v[0]),
(Scalar)bp::extract<Scalar>(v[1]),
(Scalar)bp::extract<Scalar>(v[2]));
static_cast<Scalar>(bp::extract<Scalar>(v[0])),
static_cast<Scalar>(bp::extract<Scalar>(v[1])),
static_cast<Scalar>(bp::extract<Scalar>(v[2])));
return SE3 (q.matrix(), t);
}

template <typename Vector7Like>
SE3 XYZQUATToSE3_ei(const Vector7Like& v)
{
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector7Like, v, 7, 1);
if(v.rows() != 7 || v.cols() != 1)
{
std::ostringstream shape;
shape << "(" << v.rows() << ", " << v.cols() << ")";
throw std::invalid_argument("Wrong size: v" + shape.str() + " but should have the following shape (7, 1)");
}
QuatConstMap q (v.template tail<4>().data());
return SE3 (q.matrix(), v.template head<3>());
}
Expand Down
5 changes: 5 additions & 0 deletions unittest/python/utils.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,11 @@ def test_se3ToXYZQUAT_XYZQUATToSe3(self):
m.rotation = np.array([[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.XYZQUATToSE3(np.array([1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2])), m)
with self.assertRaisesRegex(ValueError, "Wrong size: .*"):
pin.XYZQUATToSE3([1., 2., 3.])
with self.assertRaisesRegex(ValueError, "Wrong size: .*"):
pin.XYZQUATToSE3(np.array([1., 2., 3., sqrt(2) / 2]))

def test_isapprox(self):
self.assertFalse(isapprox(1, 2))
Expand Down
Loading