Commit 0eb2fdf7 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Merge remote-tracking branch 'trac/master'

parents 36a80193 f102c6cc
......@@ -20,7 +20,7 @@
#ifndef HPP_MODEL_COLLISION_OBJECT_HH
# define HPP_MODEL_COLLISION_OBJECT_HH
# include <fcl/collision_object.h>
# include <hpp/fcl/collision_object.h>
# include <hpp/util/pointer.hh>
# include <hpp/model/config.hh>
# include <hpp/model/fwd.hh>
......
......@@ -20,7 +20,7 @@
#ifndef HPP_MODEL_DISTANCE_RESULT_HH
# define HPP_MODEL_DISTANCE_RESULT_HH
# include <fcl/collision_data.h>
# include <hpp/fcl/collision_data.h>
# include <hpp/model/config.hh>
# include <hpp/model/fwd.hh>
......
......@@ -20,7 +20,7 @@
#ifndef HPP_MODEL_FCL_TO_EIGEN_HH
# define HPP_MODEL_FCL_TO_EIGEN_HH
# include <fcl/math/transform.h>
# include <hpp/fcl/math/transform.h>
# include <hpp/model/fwd.hh>
inline hpp::model::matrix_t operator* (const hpp::model::matrix_t& m1,
......
......@@ -25,8 +25,8 @@
# include <map>
# include <Eigen/Core>
# include <hpp/util/pointer.hh>
# include <fcl/fwd.hh>
# include <fcl/math/matrix_3f.h>
# include <hpp/fcl/fwd.hh>
# include <hpp/fcl/math/matrix_3f.h>
namespace hpp {
namespace model {
......@@ -51,6 +51,7 @@ namespace hpp {
typedef vector_t Configuration_t;
typedef Eigen::Ref <const Configuration_t> ConfigurationIn_t;
typedef Eigen::Ref <Configuration_t> ConfigurationOut_t;
typedef boost::shared_ptr <Configuration_t> ConfigurationPtr_t;
typedef Eigen::Ref <const vector_t> vectorIn_t;
typedef Eigen::Ref <vector_t> vectorOut_t;
typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
......
......@@ -20,7 +20,7 @@
#ifndef HPP_MANIPULATION_GRIPPER_HH
# define HPP_MANIPULATION_GRIPPER_HH
# include <fcl/math/transform.h>
# include <hpp/fcl/math/transform.h>
# include <hpp/model/joint.hh>
namespace hpp {
......
......@@ -21,7 +21,7 @@
# define HPP_MODEL_JOINT_HH
# include <cstddef>
# include <fcl/math/transform.h>
# include <hpp/fcl/math/transform.h>
# include <hpp/model/config.hh>
# include <hpp/model/fwd.hh>
......@@ -243,6 +243,40 @@ namespace hpp {
void setLinkedBody (const BodyPtr_t& body);
/// \}
/// \name Compatibility with urdf
///
/// \{
/// Get urdf link position in joint frame
///
/// When parsing urdf models, joint frames are reoriented in order
/// to rotate about their x-axis. For some applications, it is necessary
/// to be able to recover the position of the urdf link attached to
/// the joint.
const Transform3f& linkInJointFrame () const
{
return linkInJointFrame_;
}
/// Set urdf link position in joint frame
void linkInJointFrame (const Transform3f& transform)
{
linkInJointFrame_ = transform;
}
/// Get link name
const std::string& linkName () const
{
return linkName_;
}
/// Set link name
void linkName (const std::string& linkName)
{
linkName_ = linkName;
}
/// \}
/// Display joint
virtual std::ostream& display (std::ostream& os) const;
protected:
......@@ -250,6 +284,7 @@ namespace hpp {
JointConfiguration* configuration_;
mutable Transform3f currentTransformation_;
Transform3f positionInParentFrame_;
Transform3f linkInJointFrame_;
mutable Transform3f T3f_;
/// Mass of this and all descendants
value_type mass_;
......@@ -289,6 +324,7 @@ namespace hpp {
DeviceWkPtr_t robot_;
BodyPtr_t body_;
std::string name_;
std::string linkName_;
std::vector <JointPtr_t> children_;
JointPtr_t parent_;
size_type rankInConfiguration_;
......
......@@ -17,8 +17,8 @@
// hpp-model If not, see
// <http://www.gnu.org/licenses/>.
#include <fcl/distance.h>
#include <fcl/collision.h>
#include <hpp/fcl/distance.h>
#include <hpp/fcl/collision.h>
#include <hpp/util/debug.hh>
#include <hpp/model/body.hh>
#include <hpp/model/joint.hh>
......@@ -79,21 +79,11 @@ namespace hpp {
itObj != collisionInnerObjects_.end (); ++itObj) {
newBody->collisionInnerObjects_.push_back ((*itObj)->clone (joint));
}
for (ObjectVector_t::const_iterator itObj =
collisionOuterObjects_.begin ();
itObj != collisionOuterObjects_.end (); ++itObj) {
newBody->collisionOuterObjects_.push_back ((*itObj)->clone (joint));
}
for (ObjectVector_t::const_iterator itObj =
distanceInnerObjects_.begin ();
itObj != distanceInnerObjects_.end (); ++itObj) {
newBody->distanceInnerObjects_.push_back ((*itObj)->clone (joint));
}
for (ObjectVector_t::const_iterator itObj =
distanceOuterObjects_.begin ();
itObj != distanceOuterObjects_.end (); ++itObj) {
newBody->distanceOuterObjects_.push_back ((*itObj)->clone (joint));
}
return newBody;
}
......
......@@ -18,7 +18,7 @@
// <http://www.gnu.org/licenses/>.
#include <hpp/model/body.hh>
#include <fcl/math/transform.h>
#include <hpp/fcl/math/transform.h>
#include <hpp/model/joint.hh>
#include <hpp/model/gripper.hh>
......
......@@ -22,7 +22,7 @@
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <fcl/math/transform.h>
#include <hpp/fcl/math/transform.h>
#include <hpp/util/debug.hh>
#include <hpp/model/joint-configuration.hh>
......
......@@ -17,7 +17,7 @@
// hpp-model If not, see
// <http://www.gnu.org/licenses/>.
#include <fcl/math/vec_3f.h>
#include <hpp/fcl/math/vec_3f.h>
#include <hpp/util/debug.hh>
#include <hpp/model/body.hh>
#include <hpp/model/collision-object.hh>
......@@ -39,10 +39,12 @@ namespace hpp {
configSize_ (configSize), numberDof_ (numberDof),
initialPosition_ (initialPosition),
robot_ (), body_ (0x0),
name_ (), children_ (), parent_ (0x0), rankInConfiguration_ (-1),
name_ (), linkName_ (), children_ (), parent_ (0x0),
rankInConfiguration_ (-1),
jacobian_ (), rankInParent_ (0)
{
positionInParentFrame_.setIdentity ();
linkInJointFrame_.setIdentity ();
T3f_.setIdentity ();
massCom_.setValue (0);
neutralConfiguration_.resize (configSize);
......@@ -52,10 +54,11 @@ namespace hpp {
Joint::Joint (const Joint& joint) :
configuration_ (joint.configuration_),
positionInParentFrame_ (joint.positionInParentFrame_),
linkInJointFrame_ (joint.linkInJointFrame_),
maximalDistanceToParent_ (joint.maximalDistanceToParent_),
configSize_ (joint.configSize_), numberDof_ (joint.numberDof_),
robot_ (), body_ (joint.body_ ? joint.body_->clone (this) : 0x0),
name_ (joint.name_),
name_ (joint.name_), linkName_ (joint.linkName_),
children_ (), parent_ (), rankInConfiguration_ (-1), rankInVelocity_ (-1),
rankInParent_ (-1)
{
......@@ -641,9 +644,12 @@ namespace hpp {
for (hpp::model::ObjectVector_t::const_iterator it =
colObjects.begin (); it != colObjects.end (); ++it) {
os << "name: " << (*it)->name () << "\\n";
os << "position in joint:" << "\\n";
const fcl::Transform3f& local ((*it)->positionInJointFrame ());
displayTransform3f (os, local); os << "\\n";
os << "position :" << "\\n";
const fcl::Transform3f& transform ((*it)->fcl ()->getTransform ());
displayTransform3f (os, transform);
const fcl::Transform3f& global ((*it)->fcl ()->getTransform ());
displayTransform3f (os, global);
}
} else {
os << "No body";
......
Supports Markdown
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