Skip to content
Snippets Groups Projects
Commit 2032ff43 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

[wip/pinocchio] Bump to version 1.2.5

parent 9dd9bea1
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
pinocchio/Makefile
+ 1
− 2
View file @ 2032ff43
...@@ -2,7 +2,7 @@ ...@@ -2,7 +2,7 @@
# Created: Olivier Stasse on Thu, 4 Feb 2016 # Created: Olivier Stasse on Thu, 4 Feb 2016
# #
VERSION= 1.2.4 VERSION= 1.2.5
DISTNAME= pinocchio-${VERSION} DISTNAME= pinocchio-${VERSION}
MASTER_SITES= ${MASTER_SITE_OPENROBOTS:=${PKGBASE}/} MASTER_SITES= ${MASTER_SITE_OPENROBOTS:=${PKGBASE}/}
...@@ -12,7 +12,6 @@ MAINTAINER= gepetto@laas.fr ...@@ -12,7 +12,6 @@ MAINTAINER= gepetto@laas.fr
CATEGORIES= math CATEGORIES= math
COMMENT= Efficient rigid body dynamics COMMENT= Efficient rigid body dynamics
LICENSE= gnu-lgpl-v3 LICENSE= gnu-lgpl-v3
PKGREVISION= 3
# parallel builds would consume all memory because of the g++ process size # parallel builds would consume all memory because of the g++ process size
MAKE_JOBS_SAFE= no MAKE_JOBS_SAFE= no
......
pinocchio/PLIST
+ 3
− 8
View file @ 2032ff43
@comment Tue Dec 6 12:22:05 CET 2016 @comment Mon Oct 9 15:36:18 CEST 2017
include/pinocchio/algorithm/aba.hpp include/pinocchio/algorithm/aba.hpp
include/pinocchio/algorithm/aba.hxx include/pinocchio/algorithm/aba.hxx
include/pinocchio/algorithm/center-of-mass.hpp include/pinocchio/algorithm/center-of-mass.hpp
...@@ -122,16 +122,11 @@ include/pinocchio/warning.hh ...@@ -122,16 +122,11 @@ include/pinocchio/warning.hh
lib/libpinocchio.so lib/libpinocchio.so
lib/pkgconfig/pinocchio.pc lib/pkgconfig/pinocchio.pc
lib/pkgconfig/pinocchiopy.pc lib/pkgconfig/pinocchiopy.pc
${PYTHON_SITELIB}/pinocchio/libpinocchio_pywrap.so
${PYTHON_SITELIB}/pinocchio/__init__.py ${PYTHON_SITELIB}/pinocchio/__init__.py
${PYTHON_SITELIB}/pinocchio/deprecation.py
${PYTHON_SITELIB}/pinocchio/explog.py ${PYTHON_SITELIB}/pinocchio/explog.py
${PYTHON_SITELIB}/pinocchio/libpinocchio_pywrap.so
${PYTHON_SITELIB}/pinocchio/robot_wrapper.py ${PYTHON_SITELIB}/pinocchio/robot_wrapper.py
${PYTHON_SITELIB}/pinocchio/romeo_wrapper.py ${PYTHON_SITELIB}/pinocchio/romeo_wrapper.py
${PYTHON_SITELIB}/pinocchio/rpy.py ${PYTHON_SITELIB}/pinocchio/rpy.py
${PYTHON_SITELIB}/pinocchio/utils.py ${PYTHON_SITELIB}/pinocchio/utils.py
${PYTHON_SITELIB}/pinocchio/__init__.pyc
${PYTHON_SITELIB}/pinocchio/explog.pyc
${PYTHON_SITELIB}/pinocchio/robot_wrapper.pyc
${PYTHON_SITELIB}/pinocchio/romeo_wrapper.pyc
${PYTHON_SITELIB}/pinocchio/rpy.pyc
${PYTHON_SITELIB}/pinocchio/utils.pyc
pinocchio/distinfo
+ 3
− 6
View file @ 2032ff43
SHA1 (pinocchio-1.2.4.tar.gz) = 37c966e31fff360d07936012094ff5d53df5fa2c SHA1 (pinocchio-1.2.5.tar.gz) = 4a3e45b3154bf703c8822d2c3c49e32b92edf90e
RMD160 (pinocchio-1.2.4.tar.gz) = d47b71bbfaf05e6f254781915ff1a0f005a27f25 RMD160 (pinocchio-1.2.5.tar.gz) = b03d35e0f0abd1564ba8f44bb08b029dccc4dd1c
Size (pinocchio-1.2.4.tar.gz) = 8992990 bytes Size (pinocchio-1.2.5.tar.gz) = 9003067 bytes
SHA1 (patch-aa) = ef0ea5662acf1b226fee7232d6bbcc4fdae3abd9
SHA1 (patch-ab) = e517d26d988713123d1d7cb6b1badd71402329e3
SHA1 (patch-ac) = 126a46b21a39ff195bb1bad28efef51e28aed990
pinocchio/patches/patch-aa deleted 100644 → 0
+ 0
− 12
View file @ 9dd9bea1
diff --git src/parsers/srdf.hpp src/parsers/srdf.hpp
index 85eb102..34b2af4 100644
--- src/parsers/srdf.hpp
+++ src/parsers/srdf.hpp
@@ -19,6 +19,7 @@
#define __se3_parser_srdf_hpp__
#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/geometry.hpp"
#include <iostream>
// Read XML file with boost
pinocchio/patches/patch-ab deleted 100644 → 0
+ 0
− 410
View file @ 9dd9bea1
diff --git CMakeLists.txt CMakeLists.txt
index e9a253f..a0be71e 100644
--- CMakeLists.txt
+++ CMakeLists.txt
@@ -87,6 +87,11 @@ IF(EIGEN3_FOUND)
ADD_DEFINITIONS(-DEIGEN3_FUTURE)
SET(EIGEN3_FUTURE TRUE)
PKG_CONFIG_APPEND_CFLAGS("-DEIGEN3_FUTURE")
+ IF(${EIGEN3_VERSION} VERSION_LESS "3.3.0")
+ ADD_DEFINITIONS(-DEIGEN3_BETA_3_2_9x)
+ SET(EIGEN3_FUTURE TRUE)
+ PKG_CONFIG_APPEND_CFLAGS("-DEIGEN3_BETA_3_2_9x")
+ ENDIF(${EIGEN3_VERSION} VERSION_LESS "3.3.0")
ELSE(${EIGEN3_VERSION} VERSION_GREATER "3.2.10")
SET(EIGEN3_FUTURE FALSE)
ENDIF(${EIGEN3_VERSION} VERSION_GREATER "3.2.10")
@@ -310,6 +315,7 @@ SET(HEADERS
${${PROJECT_NAME}_CONTAINER_HEADERS}
exception.hpp
assert.hpp
+ macros.hpp
)
LIST(REMOVE_DUPLICATES HEADERS)
diff --git src/macros.hpp src/macros.hpp
new file mode 100644
index 0000000..689a435
--- /dev/null
+++ src/macros.hpp
@@ -0,0 +1,36 @@
+//
+// Copyright (c) 2017 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_macros_hpp__
+#define __se3_macros_hpp__
+
+#include <Eigen/Core>
+
+#ifdef EIGEN3_BETA_3_2_9x
+namespace se3
+{
+ namespace internal
+ {
+ template<typename XprType> struct size_of_xpr_at_compile_time
+ {
+ enum { ret = Eigen::internal::size_at_compile_time<Eigen::internal::traits<XprType>::RowsAtCompileTime,Eigen::internal::traits<XprType>::ColsAtCompileTime>::ret };
+ };
+ }
+}
+#endif
+
+#endif // ifndef __se3_macros_hpp__
diff --git src/multibody/constraint.hpp src/multibody/constraint.hpp
index 7965f6b..6efd2f9 100644
--- src/multibody/constraint.hpp
+++ src/multibody/constraint.hpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
@@ -20,6 +20,7 @@
#define __se3_constraint_hpp__
+#include "pinocchio/macros.hpp"
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/motion.hpp"
@@ -125,9 +126,7 @@ namespace se3
{
// There is currently a bug in Eigen/Core/util/StaticAssert.h in the use of the full namespace path
// TODO
-#ifndef EIGEN3_FUTURE
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DenseBase, D);
-#endif
}
ConstraintTpl() : S()
diff --git src/multibody/joint/joint-free-flyer.hpp src/multibody/joint/joint-free-flyer.hpp
index 64b7730..7c2781e 100644
--- src/multibody/joint/joint-free-flyer.hpp
+++ src/multibody/joint/joint-free-flyer.hpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
@@ -19,6 +19,7 @@
#ifndef __se3_joint_free_flyer_hpp__
#define __se3_joint_free_flyer_hpp__
+#include "pinocchio/macros.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
diff --git src/multibody/joint/joint-planar.hpp src/multibody/joint/joint-planar.hpp
index a03d48e..e8e49e5 100644
--- src/multibody/joint/joint-planar.hpp
+++ src/multibody/joint/joint-planar.hpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
@@ -19,6 +19,7 @@
#ifndef __se3_joint_planar_hpp__
#define __se3_joint_planar_hpp__
+#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/math/sincos.hpp"
diff --git src/multibody/joint/joint-prismatic-unaligned.hpp src/multibody/joint/joint-prismatic-unaligned.hpp
index 053df11..00f57b9 100644
--- src/multibody/joint/joint-prismatic-unaligned.hpp
+++ src/multibody/joint/joint-prismatic-unaligned.hpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
@@ -19,6 +19,7 @@
#ifndef __se3_joint_prismatic_unaligned_hpp__
#define __se3_joint_prismatic_unaligned_hpp__
+#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
diff --git src/multibody/joint/joint-prismatic.hpp src/multibody/joint/joint-prismatic.hpp
index af9d54f..50b1562 100644
--- src/multibody/joint/joint-prismatic.hpp
+++ src/multibody/joint/joint-prismatic.hpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
@@ -19,6 +19,7 @@
#ifndef __se3_joint_prismatic_hpp__
#define __se3_joint_prismatic_hpp__
+#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
diff --git src/multibody/joint/joint-revolute-unaligned.hpp src/multibody/joint/joint-revolute-unaligned.hpp
index 3f9d3b3..e0dd44e 100644
--- src/multibody/joint/joint-revolute-unaligned.hpp
+++ src/multibody/joint/joint-revolute-unaligned.hpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
@@ -19,6 +19,7 @@
#ifndef __se3_joint_revolute_unaligned_hpp__
#define __se3_joint_revolute_unaligned_hpp__
+#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
diff --git src/multibody/joint/joint-spherical-ZYX.hpp src/multibody/joint/joint-spherical-ZYX.hpp
index e00beed..9121609 100644
--- src/multibody/joint/joint-spherical-ZYX.hpp
+++ src/multibody/joint/joint-spherical-ZYX.hpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
@@ -19,6 +19,7 @@
#ifndef __se3_joint_spherical_ZYX_hpp__
#define __se3_joint_spherical_ZYX_hpp__
#include <iostream>
+#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/math/sincos.hpp"
diff --git src/multibody/joint/joint-spherical.hpp src/multibody/joint/joint-spherical.hpp
index 01b3790..8ba1744 100644
--- src/multibody/joint/joint-spherical.hpp
+++ src/multibody/joint/joint-spherical.hpp
@@ -19,6 +19,7 @@
#ifndef __se3_joint_spherical_hpp__
#define __se3_joint_spherical_hpp__
+#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/math/sincos.hpp"
diff --git src/multibody/joint/joint-translation.hpp src/multibody/joint/joint-translation.hpp
index 7a1e15d..28b1b04 100644
--- src/multibody/joint/joint-translation.hpp
+++ src/multibody/joint/joint-translation.hpp
@@ -19,6 +19,7 @@
#ifndef __se3_joint_translation_hpp__
#define __se3_joint_translation_hpp__
+#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
diff --git src/multibody/liegroup/operation-base.hxx src/multibody/liegroup/operation-base.hxx
index a596ff0..3c21836 100644
--- src/multibody/liegroup/operation-base.hxx
+++ src/multibody/liegroup/operation-base.hxx
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2016 CNRS
+// Copyright (c) 2016-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
@@ -18,6 +18,8 @@
#ifndef __se3_lie_group_operation_base_hxx__
#define __se3_lie_group_operation_base_hxx__
+#include "pinocchio/macros.hpp"
+
namespace se3 {
// --------------- API with return value as argument ---------------------- //
diff --git src/multibody/liegroup/special-euclidean.hpp src/multibody/liegroup/special-euclidean.hpp
index 1b6e378..27ba71f 100644
--- src/multibody/liegroup/special-euclidean.hpp
+++ src/multibody/liegroup/special-euclidean.hpp
@@ -20,13 +20,14 @@
#include <limits>
-#include <pinocchio/spatial/fwd.hpp>
-#include <pinocchio/spatial/se3.hpp>
-#include <pinocchio/multibody/liegroup/operation-base.hpp>
-
-#include <pinocchio/multibody/liegroup/vector-space.hpp>
-#include <pinocchio/multibody/liegroup/cartesian-product.hpp>
-#include <pinocchio/multibody/liegroup/special-orthogonal.hpp>
+#include <pinocchio/macros.hpp>
+#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/multibody/liegroup/operation-base.hpp"
+
+#include "pinocchio/multibody/liegroup/vector-space.hpp"
+#include "pinocchio/multibody/liegroup/cartesian-product.hpp"
+#include "pinocchio/multibody/liegroup/special-orthogonal.hpp"
namespace se3
{
diff --git src/spatial/act-on-set.hpp src/spatial/act-on-set.hpp
index e5b5b35..fa1b5a4 100644
--- src/spatial/act-on-set.hpp
+++ src/spatial/act-on-set.hpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2015-2016 CNRS
+// Copyright (c) 2015-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
@@ -19,7 +19,7 @@
#define __se3_act_on_set_hpp__
#include <Eigen/Core>
-#include <Eigen/Geometry>
+#include "pinocchio/macros.hpp"
#include "pinocchio/spatial/fwd.hpp"
namespace se3
diff --git src/spatial/explog.hpp src/spatial/explog.hpp
index dc94c64..f391dc3 100644
--- src/spatial/explog.hpp
+++ src/spatial/explog.hpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2015 - 2016 CNRS
+// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
@@ -21,6 +21,7 @@
# include <Eigen/Geometry>
+# include "pinocchio/macros.hpp"
# include "pinocchio/math/sincos.hpp"
# include "pinocchio/spatial/motion.hpp"
# include "pinocchio/spatial/skew.hpp"
@@ -170,4 +171,4 @@ namespace se3
}
} // namespace se3
-#endif //#ifndef __math_explog_hpp__
+#endif //#ifndef __spatial_explog_hpp__
diff --git src/spatial/force.hpp src/spatial/force.hpp
index 56f68f3..c70d3ee 100644
--- src/spatial/force.hpp
+++ src/spatial/force.hpp
@@ -20,8 +20,8 @@
#define __se3_force_hpp__
#include <Eigen/Core>
-#include <Eigen/Geometry>
#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/macros.hpp"
#include "pinocchio/spatial/se3.hpp"
/** \addtogroup Force_group Force
diff --git src/spatial/motion.hpp src/spatial/motion.hpp
index 6ba2c49..b6a588e 100644
--- src/spatial/motion.hpp
+++ src/spatial/motion.hpp
@@ -20,8 +20,8 @@
#define __se3_motion_hpp__
#include <Eigen/Core>
-#include <Eigen/Geometry>
#include "pinocchio/spatial/fwd.hpp"
+#include "pinocchio/macros.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/force.hpp"
diff --git src/spatial/se3.hpp src/spatial/se3.hpp
index 96bb2a5..625e61e 100644
--- src/spatial/se3.hpp
+++ src/spatial/se3.hpp
@@ -22,7 +22,7 @@
#include <Eigen/Geometry>
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/skew.hpp"
-
+#include "pinocchio/macros.hpp"
namespace se3
{
diff --git src/spatial/skew.hpp src/spatial/skew.hpp
index 330f5e4..186299b 100644
--- src/spatial/skew.hpp
+++ src/spatial/skew.hpp
@@ -18,6 +18,8 @@
#ifndef __se3_skew_hpp__
#define __se3_skew_hpp__
+#include "pinocchio/macros.hpp"
+
namespace se3
{
diff --git src/spatial/symmetric3.hpp src/spatial/symmetric3.hpp
index 894c5ad..4fe601f 100644
--- src/spatial/symmetric3.hpp
+++ src/spatial/symmetric3.hpp
@@ -1,5 +1,5 @@
//
-// Copyright (c) 2014_2016 CNRS
+// Copyright (c) 2014-2017 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
@@ -24,10 +24,10 @@
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
-#ifndef __se3__symmetric3_hpp__
-#define __se3__symmetric3_hpp__
+#ifndef __se3_symmetric3_hpp__
+#define __se3_symmetric3_hpp__
-#include <ostream>
+#include "pinocchio/macros.hpp"
namespace se3
{
@@ -343,5 +343,5 @@ namespace se3
} // namespace se3
-#endif // ifndef __se3__symmetric3_hpp__
+#endif // ifndef __se3_symmetric3_hpp__
pinocchio/patches/patch-ac deleted 100644 → 0
+ 0
− 169
View file @ 9dd9bea1
diff --git bindings/python/multibody/geometry-model.hpp bindings/python/multibody/geometry-model.hpp
index 06c69b8..d311af5 100644
--- bindings/python/multibody/geometry-model.hpp
+++ bindings/python/multibody/geometry-model.hpp
@@ -80,6 +80,8 @@ namespace se3
;
}
+
+
/* --- Expose --------------------------------------------------------- */
static void expose()
{
diff --git bindings/python/multibody/geometry-object.hpp bindings/python/multibody/geometry-object.hpp
index 1155c75..6e7ed0f 100644
--- bindings/python/multibody/geometry-object.hpp
+++ bindings/python/multibody/geometry-object.hpp
@@ -55,9 +55,20 @@ namespace se3
.def_readonly("meshPath", &GeometryObject::meshPath, "Absolute path to the mesh file")
.def_readonly("overrideMaterial", &GeometryObject::overrideMaterial, "Boolean that tells whether material information is stored in Geometry object")
.def_readonly("meshTexturePath", &GeometryObject::meshTexturePath, "Absolute path to the mesh texture file")
+
+ .def("CreateCapsule", &GeometryObjectPythonVisitor::maker_capsule)
+ .staticmethod("CreateCapsule")
;
}
+ static GeometryObject maker_capsule( const double radius , const double length)
+ {
+ return GeometryObject("",FrameIndex(0),JointIndex(0),
+ boost::shared_ptr<fcl::CollisionGeometry>(new fcl::Capsule (radius, length)),
+ SE3::Identity());
+
+ }
+
static void expose()
{
bp::class_<GeometryObject>("GeometryObject",
diff --git src/algorithm/frames.hpp src/algorithm/frames.hpp
index 7ac9297..cda1fc3 100644
--- src/algorithm/frames.hpp
+++ src/algorithm/frames.hpp
@@ -55,6 +55,7 @@ namespace se3
/**
* @brief Returns the jacobian of the frame expresssed in the world frame or
in the local frame depending on the template argument.
+ * You must first call se3::framesForwardKinematics and se3::computeJacobians to update values in data structure.
* @remark Expressed in the local frame, the jacobian maps the joint velocity vector to the spatial velocity of the center of the frame, expressed in the frame coordinates system. Expressed in the global frame, the jacobian maps to the spatial velocity of the point coinciding with the center of the world and attached to the frame.
*
@@ -129,20 +130,18 @@ namespace se3
const int colRef = nv(model.joints[parent])+idx_v(model.joints[parent])-1;
+ if(!local_frame)
+ getJacobian<local_frame>(model, data, parent, J);
+
// Lever between the joint center and the frame center expressed in the global frame
const SE3::Vector3 lever(data.oMi[parent].rotation() * frame.placement.translation());
-
- getJacobian<local_frame>(model, data, parent, J);
-
- if (!frame.placement.isIdentity())
+
+ for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
{
- for(int j=colRef;j>=0;j=data.parents_fromRow[(size_t) j])
- {
- if(!local_frame)
- J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
- else
- J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
- }
+ if(!local_frame)
+ J.col(j).topRows<3>() -= lever.cross(J.col(j).bottomRows<3>());
+ else
+ J.col(j) = oMframe.actInv(Motion(data.J.col(j))).toVector();
}
}
diff --git src/parsers/urdf/model.cpp src/parsers/urdf/model.cpp
index 6f0d65b..6c4bd5e 100644
--- src/parsers/urdf/model.cpp
+++ src/parsers/urdf/model.cpp
@@ -468,7 +468,7 @@ namespace se3
const Inertia YY = (!Y) ? Inertia::Zero() : convertFromUrdf(*Y);
std::cout << "Adding Body" << std::endl;
std::cout << "\"" << link_name << "\" connected to " << "\"" << parent_link_name << "\" throw joint " << "\"" << joint_name << "\"" << std::endl;
- std::cout << "joint type: " << joint_info << std::endl;
+ std::cout << "joint type: " << joint_info.str() << std::endl;
std::cout << "joint placement:\n" << jointPlacement;
std::cout << "body info: " << std::endl;
std::cout << " " << "mass: " << YY.mass() << std::endl;
diff --git unittest/frames.cpp unittest/frames.cpp
index 0d3ac4d..0fa1334 100644
--- unittest/frames.cpp
+++ unittest/frames.cpp
@@ -64,46 +64,53 @@ BOOST_AUTO_TEST_CASE ( test_jacobian )
const SE3 & framePlacement = SE3::Random();
model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
se3::Data data(model);
+ se3::Data data_ref(model);
+
VectorXd q = VectorXd::Ones(model.nq);
q.middleRows<4> (3).normalize();
VectorXd q_dot = VectorXd::Ones(model.nv);
- framesForwardKinematics(model, data, q);
-
-
- computeJacobians(model,data,q);
/// In global frame
-
Data::Matrix6x Joj(6,model.nv); Joj.fill(0);
Data::Matrix6x Jof(6,model.nv); Jof.fill(0);
Model::Index idx = model.getFrameId(frame_name);
+ const Frame & frame = model.frames[idx];
+ BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
+ computeJacobians(model,data,q);
+ computeJacobians(model,data_ref,q);
+ framesForwardKinematics(model,data);
getFrameJacobian<false>(model,data,idx,Jof);
- getJacobian<false>(model, data, parent_idx, Joj);
+ getJacobian<false>(model, data_ref, parent_idx, Joj);
Motion nu_frame(Jof*q_dot);
Motion nu_joint(Joj*q_dot);
+
+ SE3 translation(SE3::Identity());
+ translation.translation(data.oMi[parent_idx].rotation()*frame.placement.translation());
+
+ Motion nu_frame_from_nu_joint(translation.actInv(nu_joint));
- Motion nu_frame_from_nu_joint(nu_joint);
- nu_frame_from_nu_joint.linear() -= (data.oMi[parent_idx].rotation() *framePlacement.translation()).cross(nu_joint.angular());
-
-
- BOOST_CHECK(nu_frame.toVector().isApprox(nu_frame_from_nu_joint.toVector(), 1e-12));
-
+ BOOST_CHECK(nu_frame.isApprox(nu_frame_from_nu_joint, 1e-12));
/// In local frame
Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
getFrameJacobian<true>(model,data,idx,Jff);
- getJacobian<true>(model, data, parent_idx, Jjj);
-
- nu_frame = Jff*q_dot;
- nu_joint = Jjj*q_dot;
-
- BOOST_CHECK(nu_frame.toVector().isApprox(framePlacement.actInv(nu_joint).toVector(), 1e-12));
+ getJacobian<true>(model, data_ref, parent_idx, Jjj);
+
+ nu_frame = Motion(Jff*q_dot);
+ nu_joint = Motion(Jjj*q_dot);
+
+ const SE3::ActionMatrix_t jXf = frame.placement.toActionMatrix();
+ Data::Matrix6x Jjj_from_frame(jXf * Jff);
+ BOOST_CHECK(Jjj_from_frame.isApprox(Jjj));
+
+ nu_frame_from_nu_joint = frame.placement.act(nu_frame);
+ BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint), 1e-12));
}
BOOST_AUTO_TEST_SUITE_END ()
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