Skip to content

Commit

Permalink
Merge pull request #2384 from jcarpent/topic/examples
Browse files Browse the repository at this point in the history 
Restore URDF cpp example
  • Loading branch information
@jcarpent
jcarpent authored Aug 25, 2024
2 parents df7abe4 + a906bb7 commit 8d9fdbc
Show file tree
Hide file tree
Showing 3 changed files with 28 additions and 220 deletions.
1 change: 1 addition & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
- Fixed pointer casts in urdf parser ([#2339](https://github.com/stack-of-tasks/pinocchio/pull/2339))
- Remove CMake CMP0167 warnings ([#2347](https://github.com/stack-of-tasks/pinocchio/pull/2347))
- Fixed urdfdom in ROS packaging ([#2341](https://github.com/stack-of-tasks/pinocchio/pull/2341))
- Fixed overview-urdf cpp example ([#2384](https://github.com/stack-of-tasks/pinocchio/pull/2384))

### Added
- Add getMotionAxis method to helical, prismatic, revolute and ubounded revolute joint ([#2315](https://github.com/stack-of-tasks/pinocchio/pull/2315))
Expand Down
233 changes: 22 additions & 211 deletions examples/overview-urdf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,27 +2,6 @@

#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/multibody/sample-models.hpp"

#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/geometry.hpp"

#include "pinocchio/serialization/fwd.hpp"
#include "pinocchio/serialization/archive.hpp"

#include "pinocchio/serialization/spatial.hpp"

#include "pinocchio/serialization/frame.hpp"

#include "pinocchio/serialization/joints.hpp"
#include "pinocchio/serialization/model.hpp"
#include "pinocchio/serialization/data.hpp"

#include "pinocchio/serialization/geometry.hpp"

#include <boost/filesystem.hpp>

#include <iostream>

Expand All @@ -31,202 +10,34 @@
#define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
#endif

#define BOOST_CHECK(check) \
if (!(check)) \
std::cout << BOOST_STRINGIZE(check) << " has failed" << std::endl;

using namespace pinocchio;

template<typename T1, typename T2 = T1>
struct call_equality_op
int main(int argc, char ** argv)
{
static bool run(const T1 & v1, const T2 & v2)
{
return v1 == v2;
}
};

template<typename T>
bool run_call_equality_op(const T & v1, const T & v2)
{
return call_equality_op<T, T>::run(v1, v2);
}

template<typename T>
struct empty_contructor_algo
{
static T * run()
{
return new T();
}
};

template<>
struct empty_contructor_algo<pinocchio::GeometryObject>
{
static pinocchio::GeometryObject * run()
{
return new pinocchio::GeometryObject("", 0, 0, pinocchio::SE3::Identity(), nullptr);
}
};

template<typename T>
T * empty_contructor()
{
return empty_contructor_algo<T>::run();
}

template<typename T>
void generic_test(const T & object, const std::string & filename, const std::string & tag_name)
{
using namespace pinocchio::serialization;

// Load and save as TXT
const std::string txt_filename = filename + ".txt";
saveToText(object, txt_filename);

{
T & object_loaded = *empty_contructor<T>();
loadFromText(object_loaded, txt_filename);

// Check
BOOST_CHECK(run_call_equality_op(object_loaded, object));

delete &object_loaded;
}

// Load and save as string stream (TXT format)
std::stringstream ss_out;
saveToStringStream(object, ss_out);

{
T & object_loaded = *empty_contructor<T>();
std::istringstream is(ss_out.str());
loadFromStringStream(object_loaded, is);
using namespace pinocchio;

// Check
BOOST_CHECK(run_call_equality_op(object_loaded, object));

delete &object_loaded;
}

// Load and save as string
std::string str_out = saveToString(object);

{
T & object_loaded = *empty_contructor<T>();
std::string str_in(str_out);
loadFromString(object_loaded, str_in);

// Check
BOOST_CHECK(run_call_equality_op(object_loaded, object));

delete &object_loaded;
}

// Load and save as XML
const std::string xml_filename = filename + ".xml";
saveToXML(object, xml_filename, tag_name);

{
T & object_loaded = *empty_contructor<T>();
loadFromXML(object_loaded, xml_filename, tag_name);

// Check
BOOST_CHECK(run_call_equality_op(object_loaded, object));

delete &object_loaded;
}

// Load and save as binary
const std::string bin_filename = filename + ".bin";
saveToBinary(object, bin_filename);

{
T & object_loaded = *empty_contructor<T>();
loadFromBinary(object_loaded, bin_filename);

// Check
BOOST_CHECK(run_call_equality_op(object_loaded, object));

delete &object_loaded;
}

// Load and save as binary stream
boost::asio::streambuf buffer;
saveToBinary(object, buffer);

{
T & object_loaded = *empty_contructor<T>();
loadFromBinary(object_loaded, buffer);

// Check
BOOST_CHECK(run_call_equality_op(object_loaded, object));

delete &object_loaded;
}

// Load and save as static binary stream
pinocchio::serialization::StaticBuffer static_buffer(10000000);
saveToBinary(object, static_buffer);

{
T & object_loaded = *empty_contructor<T>();
loadFromBinary(object_loaded, static_buffer);

// Check
BOOST_CHECK(run_call_equality_op(object_loaded, object));

delete &object_loaded;
}
}

int main(int, char **)
{
namespace fs = boost::filesystem;
// You should change here to set up your own URDF file or just pass it as an argument of this
// example.
const std::string urdf_filename =
(argc <= 1) ? PINOCCHIO_MODEL_DIR
+ std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf")
: argv[1];

// Load the urdf model
Model model;
buildModels::humanoid(model);
Data data(model);
pinocchio::urdf::buildModel(urdf_filename, model);
std::cout << "model name: " << model.name << std::endl;

fs::path model_path = fs::temp_directory_path() / "GeometryModel";
fs::path data_path = fs::temp_directory_path() / "GeometryData";
// boost::serialization::void_cast_register<hpp::fcl::BVHModel<hpp::fcl::OBBRSS>,hpp::fcl::CollisionGeometry>();
// Empty structures
{
GeometryModel geom_model;
generic_test(geom_model, model_path.string(), "GeometryModel");

GeometryData geom_data(geom_model);
generic_test(geom_data, data_path.string(), "GeometryData");
}

#ifdef PINOCCHIO_WITH_HPP_FCL
#if HPP_FCL_VERSION_AT_LEAST(3, 0, 0)
{
pinocchio::GeometryModel geom_model;
pinocchio::buildModels::humanoidGeometries(model, geom_model);
// Append new objects
{
using namespace hpp::fcl;
BVHModel<OBBRSS> * bvh_ptr = new BVHModel<OBBRSS>();
// bvh_ptr->beginModel();
// bvh_ptr->addSubModel(p1, t1);
// bvh_ptr->endModel();
// Create data required by the algorithms
Data data(model);

GeometryObject obj_bvh(
"bvh", 0, 0, SE3::Identity(), GeometryObject::CollisionGeometryPtr(bvh_ptr));
geom_model.addGeometryObject(obj_bvh);
}
generic_test(geom_model, model_path.string(), "GeometryModel");
// Sample a random configuration
Eigen::VectorXd q = randomConfiguration(model);
std::cout << "q: " << q.transpose() << std::endl;

pinocchio::GeometryData geom_data(geom_model);
const Eigen::VectorXd q = pinocchio::neutral(model);
pinocchio::forwardKinematics(model, data, q);
pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);
// Perform the forward kinematics over the kinematic tree
forwardKinematics(model, data, q);

generic_test(geom_data, data_path.string(), "GeometryData");
}
#endif // hpp-fcl >= 3.0.0
#endif // PINOCCHIO_WITH_HPP_FCL
// Print out the placement of each joint of the kinematic tree
for (JointIndex joint_id = 0; joint_id < (JointIndex)model.njoints; ++joint_id)
std::cout << std::setw(24) << std::left << model.names[joint_id] << ": " << std::fixed
<< std::setprecision(2) << data.oMi[joint_id].translation().transpose() << std::endl;
}
14 changes: 5 additions & 9 deletions examples/simulation-pendulum-variational-casadi.ipynb
View file

Large diffs are not rendered by default.

0 comments on commit 8d9fdbc

Please sign in to comment.