Skip to content

GitLab

Commit 10ac665c authored by jcarpent's avatar jcarpent
Browse files

[C++][Doc] Clean model methods

parent 751def38
Hide whitespace changes
Inline Side-by-side
src/multibody/model.hpp
View file @ 10ac665c
......@@ -52,6 +52,7 @@ namespace se3
typedef se3::JointIndex JointIndex;
typedef se3::GeomIndex GeomIndex;
typedef se3::FrameIndex FrameIndex;
typedef std::vector<Index> IndexVector;
/// \brief Dimension of the configuration vector representation.
int nq;
......@@ -124,94 +125,51 @@ namespace se3
~Model() {} // std::cout << "Destroy model" << std::endl; }
///
/// \brief Add a Joint along with body to the kinematic tree.
/// \brief Add a joint to the kinematic tree.
///
/// \param[in] parent Index of the parent joint.
/// \param[in] j The joint model.
/// \param[in] jointPlacement The relative placement of the joint j regarding to the parent joint.
/// \param[in] Y Spatial inertia of the body.
/// \param[in] jointName Name of the joint.
/// \param[in] bodyName Name of the body.
///
/// \return The index of the new added joint.
///
template<typename D>
JointIndex addJointAndBody(JointIndex parent, const JointModelBase<D> & j, const SE3 & jointPlacement,
const Inertia & Y, const std::string & jointName = "",
const std::string & bodyName = "");
/// \remark This method also adds a Frame of same name to the vector of frames.
/// \remark The inertia supported by the joint is set to Zero
///
/// \brief Add a Joint along with body to the kinematic tree, specifying limits
/// \tparam JointModelDerived The type of the joint model.
///
/// \param[in] parent Index of the parent joint.
/// \param[in] j The joint model.
/// \param[in] jointPlacement The relative placement of the joint j regarding to the parent joint.
/// \param[in] Y Spatial inertia of the body.
/// \param[in] effort Maximal joint torque.
/// \param[in] velocity Maximal joint velocity.
/// \param[in] lowPos Lower joint configuration.
/// \param[in] upPos Upper joint configuration.
/// \param[in] jointName Name of the joint.
/// \param[in] bodyName Name of the body.
///
/// \return The index of the new added joint.
///
template<typename D>
JointIndex addJointAndBody(JointIndex parent,const JointModelBase<D> & j,const SE3 & jointPlacement,
const Inertia & Y,
const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
const std::string & jointName = "", const std::string & bodyName = "");
///
/// \brief Add a Joint with no body (Zero inertia) to the kinematic tree.
/// \param[in] joint_model The joint model.
/// \param[in] joint_placement Placement of the joint inside its parent joint.
/// \param[in] joint_name Name of the joint. If empty, the name is random.
/// \param[in] max_effort Maximal joint torque. (Default set to infinity).
/// \param[in] max_velocity Maximal joint velocity. (Default set to infinity).
/// \param[in] min_config Lower joint configuration. (Default set to infinity).
/// \param[in] max_config Upper joint configuration. (Default set to infinity).
///
/// \param[in] parent Index of the parent joint.
/// \param[in] j The joint model.
/// \param[in] jointPlacement The relative placement of the joint j regarding to the parent joint.
/// \param[in] jointName Name of the joint.
/// \return The index of the new joint.
///
/// \return The index of the new added joint.
/// \sa Model::appendBodyToJoint
///
template<typename D>
JointIndex addJoint(JointIndex parent,const JointModelBase<D> & j,const SE3 & jointPlacement,
const std::string & jointName = "");
template<typename JointModelDerived>
JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
const std::string & joint_name = "",
const Eigen::VectorXd & max_effort = Eigen::VectorXd::Constant(JointModelDerived::NV,std::numeric_limits<double>::infinity()),
const Eigen::VectorXd & max_velocity = Eigen::VectorXd::Constant(JointModelDerived::NV,std::numeric_limits<double>::infinity()),
const Eigen::VectorXd & min_config = Eigen::VectorXd::Constant(JointModelDerived::NV,-std::numeric_limits<double>::infinity()),
const Eigen::VectorXd & max_config = Eigen::VectorXd::Constant(JointModelDerived::NV,std::numeric_limits<double>::infinity())
);
///
/// \brief Add a Joint with no body (Zero inertia) to the kinematic tree, specifying limits
///
/// \param[in] parent Index of the parent joint.
/// \param[in] j The joint model.
/// \param[in] jointPlacement The relative placement of the joint j regarding to the parent joint.
/// \param[in] effort Maximal joint torque.
/// \param[in] velocity Maximal joint velocity.
/// \param[in] lowPos Lower joint configuration.
/// \param[in] upPos Upper joint configuration.
/// \param[in] jointName Name of the joint.
///
/// \return The index of the new added joint.
///
template<typename D>
JointIndex addJoint(JointIndex parent,const JointModelBase<D> & j,const SE3 & jointPlacement,
const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
const std::string & jointName = "");
/// \brief Append a body to a given joint of the kinematic tree.
///
/// \brief Append a body to a given Joint of the kinematic tree.
/// \remark This method also adds a Frame of same name to the vector of frames.
///
/// \param[in] parent Index of the parent joint.
/// \param[in] bodyPlacement The relative placement of the body regarding to the parent joint.
/// \param[in] joint_index Index of the supporting joint.
/// \param[in] Y Spatial inertia of the body.
/// \param[in] bodyName Name of the body.
/// \param[in] body_placement The relative placement of the body regarding to the parent joint. Set default to the Identity placement.
/// \param[in] body_name Name of the body. If empty, the name is random.
///
/// \sa Model::addJoint
///
void appendBodyToJoint (const JointIndex parent, const SE3 & bodyPlacement, const Inertia & Y,
const std::string & bodyName = "");
void appendBodyToJoint(const JointIndex joint_index, const Inertia & Y,
const SE3 & body_placement = SE3::Identity(),
const std::string & body_name = "");
///
/// \brief Return the index of a body given by its name.
///
......
src/multibody/model.hxx
View file @ 10ac665c
......@@ -20,13 +20,6 @@
#define __se3_model_hxx__
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
#include <iostream>
#include <boost/bind.hpp>
......@@ -45,132 +38,63 @@ namespace se3
return os;
}
template<typename D>
Model::JointIndex Model::addJointAndBody(JointIndex parent, const JointModelBase<D> & j, const SE3 & jointPlacement,
const Inertia & Y, const std::string & jointName,
const std::string & bodyName)
{
JointIndex idx = addJoint(parent,j,jointPlacement,jointName);
appendBodyToJoint(idx, SE3::Identity(), Y, bodyName);
return idx;
}
template<typename D>
Model::JointIndex Model::addJointAndBody(JointIndex parent, const JointModelBase<D> & j, const SE3 & jointPlacement,
const Inertia & Y,
const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
const std::string & jointName,
const std::string & bodyName)
{
JointIndex idx = addJoint(parent,j,jointPlacement,
effort, velocity, lowPos, upPos,
jointName);
appendBodyToJoint(idx, SE3::Identity(), Y, bodyName);
return idx;
}
template<typename D>
Model::JointIndex Model::addJoint(JointIndex parent, const JointModelBase<D> & j, const SE3 & jointPlacement,
const std::string & jointName)
template<typename JointModelDerived>
Model::JointIndex Model::addJoint(const Model::JointIndex parent,
const JointModelBase<JointModelDerived> & joint_model,
const SE3 & joint_placement,
const std::string & joint_name,
const Eigen::VectorXd & max_effort,
const Eigen::VectorXd & max_velocity,
const Eigen::VectorXd & min_config,
const Eigen::VectorXd & max_config
)
{
typedef JointModelDerived D;
assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
&&(njoint==(int)parents.size())&&(njoint==(int)jointPlacements.size()) );
assert( (j.nq()>=0)&&(j.nv()>=0) );
Model::JointIndex idx = (Model::JointIndex) (njoint ++);
joints .push_back(JointModel(j.derived()));
boost::get<D>(joints.back()).setIndexes(idx,nq,nv);
inertias .push_back(Inertia::Zero());
parents .push_back(parent);
jointPlacements.push_back(jointPlacement);
names .push_back( (jointName!="")?jointName:random(8) );
nq += j.nq();
nv += j.nv();
neutralConfiguration.conservativeResize(nq);
neutralConfiguration.bottomRows<D::NQ>() = j.neutralConfiguration();
effortLimit.conservativeResize(nv);
effortLimit.bottomRows<D::NV>().fill(std::numeric_limits<double>::infinity());
velocityLimit.conservativeResize(nv);
velocityLimit.bottomRows<D::NV>().fill(std::numeric_limits<double>::infinity());
lowerPositionLimit.conservativeResize(nq);
lowerPositionLimit.bottomRows<D::NQ>().fill(-std::numeric_limits<double>::infinity());
upperPositionLimit.conservativeResize(nq);
upperPositionLimit.bottomRows<D::NQ>().fill(std::numeric_limits<double>::infinity());
return idx;
}
template<typename D>
Model::JointIndex Model::addJoint(JointIndex parent,const JointModelBase<D> & j,const SE3 & jointPlacement,
const Eigen::VectorXd & effort, const Eigen::VectorXd & velocity,
const Eigen::VectorXd & lowPos, const Eigen::VectorXd & upPos,
const std::string & jointName)
{
assert( (njoint==(int)joints.size())&&(njoint==(int)inertias.size())
&&(njoint==(int)parents.size())&&(njoint==(int)jointPlacements.size()) );
assert( (j.nq()>=0)&&(j.nv()>=0) );
assert( effort.size() == j.nv() && velocity.size() == j.nv()
&& lowPos.size() == j.nq() && upPos.size() == j.nq() );
Model::JointIndex idx = (Model::JointIndex) (njoint ++);
joints .push_back(JointModel(j.derived()));
boost::get<D>(joints.back()).setIndexes(idx,nq,nv);
&&(njoint==(int)parents.size())&&(njoint==(int)jointPlacements.size()) );
assert((joint_model.nq()>=0) && (joint_model.nv()>=0));
assert(max_effort.size() == joint_model.nv()
&& max_velocity.size() == joint_model.nv()
&& min_config.size() == joint_model.nq()
&& max_config.size() == joint_model.nq());
Model::JointIndex idx = (Model::JointIndex) (njoint++);
joints .push_back(JointModel(joint_model.derived()));
boost::get<JointModelDerived>(joints.back()).setIndexes(idx,nq,nv);
inertias .push_back(Inertia::Zero());
parents .push_back(parent);
jointPlacements.push_back(jointPlacement);
names .push_back( (jointName!="")?jointName:random(8) );
nq += j.nq();
nv += j.nv();
jointPlacements.push_back(joint_placement);
names .push_back((joint_name!="")?joint_name:randomStringGenerator(8));
nq += joint_model.nq();
nv += joint_model.nv();
effortLimit.conservativeResize(nv);effortLimit.bottomRows<D::NV>() = effort;
velocityLimit.conservativeResize(nv);velocityLimit.bottomRows<D::NV>() = velocity;
lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows<D::NQ>() = lowPos;
upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows<D::NQ>() = upPos;
// Ensure that limits are not inf
Eigen::VectorXd neutralConf((lowerPositionLimit.bottomRows<D::NQ>() + upperPositionLimit.bottomRows<D::NQ>())/2 );
effortLimit.conservativeResize(nv);effortLimit.bottomRows<D::NV>() = max_effort;
velocityLimit.conservativeResize(nv);velocityLimit.bottomRows<D::NV>() = max_velocity;
lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows<D::NQ>() = min_config;
upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows<D::NQ>() = max_config;
neutralConfiguration.conservativeResize(nq);
if ( std::isfinite(neutralConf.norm()) )
{
neutralConfiguration.bottomRows<D::NQ>() = neutralConf;
}
else
{
assert( false && "One of the position limit is inf or NaN");
neutralConfiguration.bottomRows<D::NQ>() = j.neutralConfiguration();
}
addFrame((jointName!="")?jointName:random(8), idx, SE3::Identity(), JOINT);
neutralConfiguration.tail(joint_model.nq()) = joint_model.neutralConfiguration();
// Add a the joint frame attached to itself to the frame vector - redundant information but useful.
addFrame(names[idx],idx,SE3::Identity(),JOINT);
return idx;
}
inline void Model::appendBodyToJoint(const JointIndex parent, const SE3 & bodyPlacement, const Inertia & Y,
const std::string & bodyName)
inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
const Inertia & Y,
const SE3 & body_placement,
const std::string & body_name)
{
const Inertia & iYf = Y.se3Action(bodyPlacement);
inertias[parent] += iYf;
addFrame((bodyName!="")?bodyName:random(8), parent, bodyPlacement, BODY);
const Inertia & iYf = Y.se3Action(body_placement);
inertias[joint_index] += iYf;
nbody ++;
addFrame((body_name!="")?body_name:randomStringGenerator(8), joint_index, body_placement, BODY);
nbody++;
}
inline Model::JointIndex Model::getBodyId (const std::string & name) const
......
src/parsers/urdf.hpp
View file @ 10ac665c
......@@ -19,25 +19,17 @@
#ifndef __se3_parsers_urdf_hpp__
#define __se3_parsers_urdf_hpp__
#include <urdf_model/model.h>
#include <urdf_parser/urdf_parser.h>
#include <iostream>
#include <sstream>
#include <iomanip>
#include <boost/foreach.hpp>
#include "pinocchio/multibody/model.hpp"
#include <exception>
#include <limits>
#ifdef WITH_HPP_FCL
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include "pinocchio/multibody/geometry.hpp"
#include "pinocchio/multibody/geometry.hpp"
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#endif
#include <string>
#include <exception>
namespace urdf
{
typedef boost::shared_ptr<ModelInterface> ModelInterfacePtr;
......
src/parsers/urdf/geometry.hxx
View file @ 10ac665c
//
// Copyright (c) 2015 - 2016 CNRS
// Copyright (c) 2015-2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
......
Markdown is supported
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