From 2d73f6a23784769a7bd4766eb43214c95fd4ba25 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Mon, 25 Aug 2014 00:12:51 +0200
Subject: [PATCH] IVIT value test: check the numerical value of RNEA(0,0,0)
against RBDL for simple humanoid.
---
CMakeLists.txt | 5 ++
src/algorithm/kinematics.hpp | 71 ++++++++++++++++++++++++++
src/algorithm/rnea.hpp | 4 +-
src/multibody/joint/joint-revolute.hpp | 2 +-
src/multibody/model.hpp | 2 +-
unittest/value.cpp | 47 +++++++++++++++++
6 files changed, 127 insertions(+), 4 deletions(-)
create mode 100644 src/algorithm/kinematics.hpp
create mode 100644 unittest/value.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b5759b14b..5c8fca6b1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -48,6 +48,7 @@ SET(${PROJECT_NAME}_HEADERS
multibody/parser/urdf.hpp
tools/timer.hpp
algorithm/rnea.hpp
+ algorithm/kinematics.hpp
)
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio")
@@ -90,6 +91,10 @@ ADD_EXECUTABLE(urdf EXCLUDE_FROM_ALL unittest/urdf.cpp)
PKG_CONFIG_USE_DEPENDENCY(urdf eigenpy)
PKG_CONFIG_USE_DEPENDENCY(urdf urdfdom)
+ADD_EXECUTABLE(value EXCLUDE_FROM_ALL unittest/value.cpp)
+PKG_CONFIG_USE_DEPENDENCY(value eigenpy)
+PKG_CONFIG_USE_DEPENDENCY(value urdfdom)
+
ADD_EXECUTABLE(variant EXCLUDE_FROM_ALL unittest/variant.cpp)
PKG_CONFIG_USE_DEPENDENCY(variant eigenpy)
diff --git a/src/algorithm/kinematics.hpp b/src/algorithm/kinematics.hpp
new file mode 100644
index 000000000..37e0c2ea4
--- /dev/null
+++ b/src/algorithm/kinematics.hpp
@@ -0,0 +1,71 @@
+#ifndef __se3_kinematics_hpp__
+#define __se3_kinematics_hpp__
+
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/multibody/model.hpp"
+
+namespace se3
+{
+ inline void kinematics(const Model & model,
+ Data& data,
+ const Eigen::VectorXd & q,
+ const Eigen::VectorXd & v);
+} // namespace se3
+
+/* --- Details -------------------------------------------------------------------- */
+namespace se3
+{
+ struct KinematicsStep : public fusion::JointVisitor<KinematicsStep>
+ {
+ typedef boost::fusion::vector< const se3::Model&,
+ se3::Data&,
+ const int&,
+ const Eigen::VectorXd &,
+ const Eigen::VectorXd &
+ > ArgsType;
+
+ JOINT_VISITOR_INIT(KinematicsStep);
+
+ template<typename JointModel>
+ static void algo(const se3::JointModelBase<JointModel> & jmodel,
+ se3::JointDataBase<typename JointModel::JointData> & jdata,
+ const se3::Model& model,
+ se3::Data& data,
+ const int &i,
+ const Eigen::VectorXd & q,
+ const Eigen::VectorXd & v)
+ {
+ using namespace Eigen;
+ using namespace se3;
+
+ jmodel.calc(jdata.derived(),q,v);
+
+ const Model::Index & parent = model.parents[i];
+ data.liMi[i] = model.jointPlacements[i]*jdata.M();
+
+ if(parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
+ else data.oMi[i] = data.liMi[i];
+
+ data.v[i] = jdata.v();
+ if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]);
+ }
+
+ };
+
+ inline void
+ kinematics(const Model & model, Data& data,
+ const Eigen::VectorXd & q,
+ const Eigen::VectorXd & v)
+ {
+ data.v[0] = Motion::Zero();
+
+ for( int i=1;i<model.nbody;++i )
+ {
+ KinematicsStep::run(model.joints[i],data.joints[i],
+ KinematicsStep::ArgsType(model,data,i,q,v));
+ }
+ }
+} // namespace se3
+
+#endif // ifndef __se3_kinematics_hpp__
+
diff --git a/src/algorithm/rnea.hpp b/src/algorithm/rnea.hpp
index 4288093ce..bee46880b 100644
--- a/src/algorithm/rnea.hpp
+++ b/src/algorithm/rnea.hpp
@@ -53,9 +53,9 @@ namespace se3
data.v[i] = jdata.v();
if(parent>0) data.v[i] += data.liMi[i].actInv(data.v[parent]);
- jmodel.jointMotion(a);
data.a[i] = jdata.S()*jmodel.jointMotion(a) + jdata.c() + (data.v[i] ^ jdata.v()) ;
- if(parent>0) data.a[i] += data.liMi[i].actInv(data.a[parent]);
+ // if(parent>0)
+ data.a[i] += data.liMi[i].actInv(data.a[parent]);
data.f[i] = model.inertias[i]*data.a[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
return 0;
diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp
index 22072d30e..71db05102 100644
--- a/src/multibody/joint/joint-revolute.hpp
+++ b/src/multibody/joint/joint-revolute.hpp
@@ -61,7 +61,7 @@ namespace se3
//template<typename D> D operator*( const Force& f ) const
Force::Vector3::ConstFixedSegmentReturnType<1>::Type
operator*( const Force& f ) const
- { return f.angular().head<1>(); }
+ { return f.angular().segment<1>(axis); }
}; // struct ConstraintRevolute
static Eigen::Matrix3d cartesianRotation(const double & angle);
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index 2f4fdec9b..cf53f9363 100644
--- a/src/multibody/model.hpp
+++ b/src/multibody/model.hpp
@@ -76,7 +76,7 @@ namespace se3
};
- const Eigen::Vector3d Model::gravity981 (0,0,9.81);
+ const Eigen::Vector3d Model::gravity981 (0,0,-9.81);
} // namespace se3
diff --git a/unittest/value.cpp b/unittest/value.cpp
new file mode 100644
index 000000000..843f2f5bf
--- /dev/null
+++ b/unittest/value.cpp
@@ -0,0 +1,47 @@
+#include <iostream>
+#include <iomanip>
+
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/multibody/parser/urdf.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+
+int main(int argc, const char**argv)
+{
+ std::string filename = "/home/nmansard/src/rbdl/rbdl_evaluate_performances/models/simple_humanoid.urdf";
+ if(argc>1) filename = argv[1];
+
+ se3::Model model = se3::buildModel(filename);
+ model.gravity.linear( Eigen::Vector3d(0,0,9.8));
+ se3::Data data(model);
+
+ Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
+ Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
+ Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
+
+ //kinematics(model,data,q,v);
+ rnea(model,data,q,v,a);
+
+ using namespace Eigen;
+ using namespace se3;
+
+ std::cout << std::setprecision(10);
+
+ std::cout << "Number of dof : " << model.nv << std::endl;
+ std::cout << "rnea(0,0,0) = g(0) = " << data.tau.transpose() << std::endl;
+
+ for( int i=0;i<model.nbody;++i )
+ {
+ if(model.parents[i]!=i-1)
+ std::cout << "************** END EFFECTOR" << std::endl;
+
+ std::cout << "\n\n === " << i << " ========================" << std::endl;
+ std::cout << "Joint "<<i<<" = " << model.names[i] << std::endl;
+ std::cout << "m"<<i<<" = \n" << (SE3::Matrix4)data.oMi[i] << std::endl;
+ std::cout << "v"<<i<<" = \n" << SE3::Vector6(data.v[i]).transpose()<< std::endl;
+ std::cout << "a"<<i<<" = \n" << SE3::Vector6(data.a[i]).transpose() << std::endl;
+ std::cout << "f"<<i<<" = \n" << SE3::Vector6(data.f[i]).transpose() << std::endl;
+ }
+
+ return 0;
+}
--
GitLab