Skip to content
GitLab
Commit 46604017 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

Move class LockedJoint to hpp-constraints.

parent b7763db3
Hide whitespace changes
Inline Side-by-side
include/hpp/core/constraint-set.hh
View file @ 46604017
......@@ -157,7 +157,7 @@ namespace hpp {
Constraints_t::iterator trivialOrNotConfigProjectorIt_;
ConstraintSetWkPtr_t weak_;
friend class LockedJoint;
friend class constraints::LockedJoint;
friend class Constraint;
friend class ConfigProjector;
}; // class ConstraintSet
......
include/hpp/core/constraint.hh
View file @ 46604017
......@@ -91,7 +91,7 @@ namespace hpp {
std::string name_;
ConstraintWkPtr_t weak_;
friend class ConstraintSet;
friend class LockedJoint;
friend class constraints::LockedJoint;
friend class ConfigProjector;
friend std::ostream& operator<< (std::ostream& os, const Constraint&);
}; // class Constraint
......
include/hpp/core/fwd.hh
View file @ 46604017
......@@ -51,7 +51,6 @@ namespace hpp {
HPP_PREDEF_CLASS (DiscretizedPathValidation);
HPP_PREDEF_CLASS (PathValidations);
HPP_PREDEF_CLASS (ExplicitRelativeTransformation);
HPP_PREDEF_CLASS (LockedJoint);
class Edge;
HPP_PREDEF_CLASS (ExtractedPath);
HPP_PREDEF_CLASS (SubchainPath);
......@@ -153,8 +152,9 @@ namespace hpp {
typedef pinocchio::HalfJointJacobian_t HalfJointJacobian_t;
typedef pinocchio::JointVector_t JointVector_t;
typedef KDTree* KDTreePtr_t;
typedef boost::shared_ptr <LockedJoint> LockedJointPtr_t;
typedef boost::shared_ptr <const LockedJoint> LockedJointConstPtr_t;
typedef constraints::LockedJoint LockedJoint;
typedef constraints::LockedJointPtr_t LockedJointPtr_t;
typedef constraints::LockedJointConstPtr_t LockedJointConstPtr_t;
typedef std::vector <LockedJointPtr_t> LockedJoints_t;
typedef pinocchio::matrix_t matrix_t;
typedef pinocchio::matrix3_t matrix3_t;
......
include/hpp/core/locked-joint.hh
View file @ 46604017
// Copyright (c) 2015, LAAS-CNRS
// Copyright (c) 2015 - 2018, LAAS-CNRS
// Authors: Florent Lamiraux, Joseph Mirabel
//
// This file is part of hpp-core.
......@@ -18,169 +18,9 @@
#ifndef HPP_CORE_LOCKED_JOINT_HH
# define HPP_CORE_LOCKED_JOINT_HH
# include <hpp/pinocchio/joint.hh>
# warning "This file is deprecated. Include <hpp/constraints/locked-joint.hh instead."
# include <hpp/constraints/explicit.hh>
# include <hpp/constraints/locked-joint.hh>
# include <hpp/core/config.hh>
# include <hpp/core/fwd.hh>
namespace hpp {
namespace core {
typedef constraints::Implicit Implicit;
typedef constraints::ImplicitPtr_t ImplicitPtr_t;
/// \addtogroup constraints
/// \{
/**
Implementation of constraint specific to locked joint.
The underlying equation is \f$ q_i (q) = rhs \f$.
The right hand side of the equation is also called value.
*/
class HPP_CORE_DLLAPI LockedJoint : public constraints::Explicit
{
public:
/// Copy object and return shared pointer to copy
virtual ImplicitPtr_t copy () const;
/// Create locked joint and return shared pointer
/// \param joint joint that is locked,
/// \param value of the constant joint config,
static LockedJointPtr_t create (const JointPtr_t& joint,
const LiegroupElement& value);
/// Create partial locked joint (only some degrees of freedom)
/// \param joint joint that is locked,
/// \param index first locked degree of freedom in the joint,
/// \param value of the constant joint partial config, size of value
/// determines the number of degrees of freedom locked.
/// \note valid only for translation joints.
static LockedJointPtr_t create (const JointPtr_t& joint,
const size_type index,
vectorIn_t value);
/// Create locked degrees of freedom of extra config space
/// \param robot robot
/// \param index index of the first locked extra degree of freedom,
/// \param value of the locked degrees of freedom, size of value
/// determines the number of degrees of freedom locked.
static LockedJointPtr_t create (const DevicePtr_t& dev,
const size_type index,
vectorIn_t value);
/// Return shared pointer to copy
/// \param other instance to copy.
static LockedJointPtr_t createCopy (LockedJointConstPtr_t other);
/// Get index of locked degree of freedom in robot configuration vector
size_type rankInConfiguration () const;
/// Get index of locked degree of freedom in robot velocity vector.
size_type rankInVelocity () const;
/// Get the configuration size of the joint.
size_type configSize () const;
/// Get number of degrees of freedom of the joint
size_type numberDof () const;
/// Get output configuration variables
segments_t outputConf () const
{
return segments_t(1, segment_t (rankInConfiguration(), configSize()));
}
/// Get output degrees of freedom
segments_t outputVelocity () const
{
return segments_t(1, segment_t (rankInVelocity(), numberDof()));
}
/// Get input configuration variables
segments_t inputConf () const
{
return segments_t();
}
/// Get input degrees of freedom
segments_t inputVelocity () const
{
return segments_t();
}
/// Get configuration space of locked joint
const LiegroupSpacePtr_t& configSpace () const;
/// Get the value of the locked joint.
vectorIn_t value () const;
/// Set the value of the locked joint.
void value (vectorIn_t value);
/// Set the value of the locked joint from a configuration.
void rightHandSideFromConfig (ConfigurationIn_t config);
/// Check whether a configuration statisfies the constraint.
bool isSatisfied (ConfigurationIn_t config);
/// Check whether a configuration statisfies the constraint.
///
/// \param config input configuration
/// \retval error difference between configuration of joint read in
/// input configuration and locked value.
bool isSatisfied (ConfigurationIn_t config, vector_t& error);
/// Return shared pointer to joint
const JointPtr_t& joint ()
{
return joint_;
}
/// Return the joint name.
const std::string& jointName () const {
return jointName_;
}
/// Print object in a stream
std::ostream& print (std::ostream& os) const;
protected:
/// Constructor
/// \param joint joint that is locked,
/// \param value of the constant joint config,
LockedJoint (const JointPtr_t& joint, const LiegroupElement& value);
/// Constructor of partial locked joint
/// \param joint joint that is locked,
/// \param index first locked degree of freedom in the joint,
/// \param value of the constant joint partial config, size of value
/// determines the number of degrees of freedom locked.
/// \note valid only for translation joints.
LockedJoint (const JointPtr_t& joint, const size_type index,
vectorIn_t value);
/// Constructor of locked degrees of freedom of extra config space
/// \param robot robot
/// \param index index of the first locked extra degree of freedom,
/// \param value of the locked degrees of freedom, size of value
/// determines the number of degrees of freedom locked.
LockedJoint (const DevicePtr_t& robot, const size_type index,
vectorIn_t value);
/// Copy constructor
LockedJoint (const LockedJoint& other);
/// Test equality with other instance
/// \param other object to copy
/// \param swapAndTest whether we should also check other == this
virtual bool isEqual (const Implicit& other, bool swapAndTest) const;
void init (const LockedJointPtr_t& self);
private:
std::string jointName_;
JointPtr_t joint_;
LiegroupSpacePtr_t configSpace_;
/// Weak pointer to itself
LockedJointWkPtr_t weak_;
friend class ConfigProjector;
}; // class LockedJoint
/// \}
inline std::ostream& operator<< (std::ostream& os, const LockedJoint& lj)
{
return lj.print (os);
}
} // namespace core
} // namespace hpp
#endif // HPP_CORE_LOCKED_JOINT_HH
#endif //HPP_CORE_LOCKED_JOINT_HH
src/CMakeLists.txt
View file @ 46604017
......@@ -46,7 +46,6 @@ explicit-relative-transformation.cc
extracted-path.hh
interpolated-path.cc
joint-bound-validation.cc
locked-joint.cc
path-validations.cc
nearest-neighbor/basic.hh #
nearest-neighbor/basic.cc #
......
src/config-projector.cc
View file @ 46604017
......@@ -39,7 +39,7 @@
#include <hpp/constraints/solver/by-substitution.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/core/locked-joint.hh>
#include <hpp/constraints/locked-joint.hh>
#include <hpp/constraints/explicit.hh>
#include <hpp/constraints/implicit.hh>
......@@ -399,7 +399,7 @@ namespace hpp {
{
throw std::runtime_error
("Could not replace lockedJoint function " +
lockedJoint->jointName_);
lockedJoint->jointName ());
}
*itLock = lockedJoint;
return;
......@@ -417,7 +417,8 @@ namespace hpp {
types) >= 0;
if (!added) {
throw std::runtime_error("Could not add lockedJoint function " + lockedJoint->jointName_);
throw std::runtime_error("Could not add lockedJoint function " +
lockedJoint->jointName ());
}
if (added) {
solver_->explicitConstraintSet().rightHandSide (
......@@ -427,7 +428,7 @@ namespace hpp {
solver_->explicitConstraintSetHasChanged();
lockedJoints_.push_back (lockedJoint);
hppDout (info, "add locked joint " << lockedJoint->jointName_
hppDout (info, "add locked joint " << lockedJoint->jointName ()
<< " rank in velocity: " << lockedJoint->rankInVelocity ()
<< ", size: " << lockedJoint->numberDof ());
hppDout (info, "Intervals: "
......
src/locked-joint.cc deleted 100644 → 0
View file @ b7763db3
// Copyright (c) 2015, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core 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.
//
// hpp-core 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
// hpp-core. If not, see <http://www.gnu.org/licenses/>.
#include "hpp/core/locked-joint.hh"
#include <sstream>
#include <boost/assign/list_of.hpp>
#include <hpp/util/debug.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/constraints/affine-function.hh>
namespace hpp {
namespace core {
using boost::assign::list_of;
using constraints::ConstantFunction;
typedef constraints::Implicit Implicit;
typedef constraints::ImplicitPtr_t ImplicitPtr_t;
namespace {
template <typename T>
std::string numToStr (const T& v) {
std::stringstream ss; ss<<v; return ss.str ();
}
DifferentiableFunctionPtr_t makeFunction (
const LiegroupElement& lge, const std::string& name)
{
return DifferentiableFunctionPtr_t
(new ConstantFunction (lge, 0, 0, "LockedJoint " + name));
}
}
/// Copy object and return shared pointer to copy
ImplicitPtr_t LockedJoint::copy () const
{
return createCopy (weak_.lock ());
}
LockedJointPtr_t LockedJoint::create (const JointPtr_t& joint,
const LiegroupElement& value)
{
LockedJoint* ptr = new LockedJoint (joint, value);
LockedJointPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
LockedJointPtr_t LockedJoint::create (const JointPtr_t& joint,
const size_type index,
vectorIn_t value)
{
LockedJoint* ptr = new LockedJoint (joint, index, value);
LockedJointPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
LockedJointPtr_t LockedJoint::create (const DevicePtr_t& dev,
const size_type index, vectorIn_t value)
{
LockedJoint* ptr = new LockedJoint (dev, index, value);
LockedJointPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
LockedJointPtr_t LockedJoint::createCopy (LockedJointConstPtr_t other)
{
LockedJoint* ptr = new LockedJoint (*other);
LockedJointPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
size_type LockedJoint::rankInConfiguration () const
{
return outputConf_[0].first;
}
size_type LockedJoint::rankInVelocity () const
{
return outputVelocity_[0].first;
}
size_type LockedJoint::configSize () const
{
return configSpace_->nq ();
}
size_type LockedJoint::numberDof () const
{
return configSpace_->nv ();
}
const LiegroupSpacePtr_t& LockedJoint::configSpace () const
{
return configSpace_;
}
bool LockedJoint::isSatisfied (ConfigurationIn_t config)
{
vector_t error;
return isSatisfied (config, error);
}
bool LockedJoint::isSatisfied (ConfigurationIn_t config, vector_t& error)
{
LiegroupElement q (config.segment (rankInConfiguration(), configSize ()),
configSpace_);
error = q - (configSpace_->exp (rightHandSide ()));
return error.isApprox (vector_t::Zero (joint_->numberDof ()));
}
void LockedJoint::rightHandSideFromConfig (ConfigurationIn_t config)
{
if (!constantRightHandSide ()) {
LiegroupElement q (config.segment (rankInConfiguration(), configSize ()),
configSpace_);
rightHandSide (q - configSpace_->neutral ());
}
}
LockedJoint::LockedJoint (const JointPtr_t& joint,
const LiegroupElement& value) :
constraints::Explicit (
joint->robot(),
makeFunction (value, joint->name()),
segments_t(), // input conf
segments_t(), // input vel
list_of(segment_t (joint->rankInConfiguration(), joint->configSize())), // output conf
list_of(segment_t (joint->rankInVelocity (), joint->numberDof ())), // output vel
ComparisonTypes_t(joint->numberDof(), constraints::Equality)),
jointName_ (joint->name ()),
configSpace_ (joint->configurationSpace ())
{
assert (rhsSize () == joint->numberDof ());
assert (*(value.space ()) == *configSpace_);
// rightHandSide (value - configSpace_->neutral ());
}
LockedJoint::LockedJoint (const JointPtr_t& joint, const size_type index,
vectorIn_t value) :
constraints::Explicit (
joint->robot(),
makeFunction (
LiegroupElement (value, LiegroupSpace::Rn (joint->configSize () - index)),
"partial_" + joint->name()),
segments_t(), // input conf
segments_t(), // input vel
list_of(segment_t (joint->rankInConfiguration(), joint->configSize()-index)), // output conf
list_of(segment_t (joint->rankInVelocity (), joint->numberDof ()-index)), // output vel
ComparisonTypes_t(joint->numberDof()-index, constraints::Equality)),
jointName_ ("partial_" + joint->name ()),
joint_ (joint),
configSpace_ (LiegroupSpace::Rn (joint->configSize () - index))
{
assert (joint->numberDof () == joint->configSize ());
// rightHandSide (value);
assert (rhsSize () == value.size());
}
LockedJoint::LockedJoint (const DevicePtr_t& dev, const size_type index,
vectorIn_t value) :
constraints::Explicit (
dev,
makeFunction (
LiegroupElement (value, LiegroupSpace::Rn (value.size ())),
dev->name() + "_extraDof" + numToStr (index)),
segments_t(), // input conf
segments_t(), // input vel
list_of(segment_t (
dev->configSize () - dev->extraConfigSpace().dimension() + index,
value.size())), // output conf
list_of(segment_t (
dev->numberDof () - dev->extraConfigSpace().dimension() + index,
value.size())), // output vel
ComparisonTypes_t(value.size(), constraints::Equality)),
jointName_ (dev->name() + "_extraDof" + numToStr (index)),
joint_ (JointPtr_t ()), configSpace_ (LiegroupSpace::Rn (value.size ()))
{
assert (value.size() > 0);
assert (rankInConfiguration() + value.size() <= dev->configSize());
// rightHandSide (value);
assert (rhsSize () == value.size());
}
void LockedJoint::init (const LockedJointPtr_t& self)
{
constraints::Explicit::init(self);
weak_ = self;
}
std::ostream& LockedJoint::print (std::ostream& os) const
{
LiegroupElement v; vector_t empty;
function().value(v, empty);
v += rightHandSide();
os << "Locked joint " << jointName_
<< ", value = " << pinocchio::displayConfig (v.vector())
<< ": rank in configuration = " << rankInConfiguration()
<< ": rank in velocity = " << rankInVelocity()
<< std::endl;
return os;
}
LockedJoint::LockedJoint (const LockedJoint& other) :
constraints::Explicit (other), jointName_ (other.jointName_),
joint_ (other.joint_), configSpace_ (other.configSpace_), weak_ ()
{
}
bool LockedJoint::isEqual (const Implicit& other, bool swapAndTest) const
{
try {
const LockedJoint& lj =
dynamic_cast <const LockedJoint&> (other);
if (!Implicit::isEqual (other, false)) return false;
if (jointName_ != lj.jointName_) return false;
if (rankInConfiguration() != lj.rankInConfiguration()) return false;
if (rankInVelocity() != lj.rankInVelocity()) return false;
if (*configSpace_ != *(lj.configSpace_)) return false;
if (swapAndTest) return lj.isEqual (*this, false);
return true;
} catch (const std::bad_cast& err) {
return false;
}
}
} // namespace core
} // namespace hpp
src/path-optimization/partial-shortcut.cc
View file @ 46604017
......@@ -30,7 +30,7 @@
#include <hpp/core/path-vector.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/core/locked-joint.hh>
#include <hpp/constraints/locked-joint.hh>
namespace hpp {
namespace core {
......
src/problem-solver.cc
View file @ 46604017
......@@ -45,7 +45,7 @@
#include <hpp/core/distance/reeds-shepp.hh>
#include <hpp/core/distance-between-objects.hh>
#include <hpp/core/discretized-collision-checking.hh>
#include <hpp/core/locked-joint.hh>
#include <hpp/constraints/locked-joint.hh>
#include <hpp/constraints/implicit.hh>
#include <hpp/core/path-planner/k-prm-star.hh>
#include <hpp/core/path-projector/global.hh>
......
src/relative-motion.cc
View file @ 46604017
......@@ -27,7 +27,7 @@
#include <hpp/core/constraint-set.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/constraints/implicit.hh>
#include <hpp/core/locked-joint.hh>
#include <hpp/constraints/locked-joint.hh>
#include <hpp/constraints/explicit/function.hh>
......
tests/explicit-relative-transformation.cc
View file @ 46604017
......@@ -92,7 +92,7 @@ BOOST_AUTO_TEST_CASE (two_freeflyers)
ExplicitRelativeTransformationPtr_t ert = ExplicitRelativeTransformation::create (
"explicit_relative_transformation", robot,
object1, object2, M1inO1, M2inO2);
constraints::ExplicitPtr_t enm = ert->createNumericalConstraint();
ExplicitPtr_t enm = ert->createNumericalConstraint();
Configuration_t q = robot->currentConfiguration (),
qrand = se3::randomConfiguration(robot->model()),
......@@ -186,7 +186,7 @@ BOOST_AUTO_TEST_CASE (two_frames_on_freeflyer)
ExplicitRelativeTransformationPtr_t ert = ExplicitRelativeTransformation::create (
"explicit_relative_transformation", robot,
object1, object2, M1inO1, M2inO2);
constraints::ExplicitPtr_t enm = ert->createNumericalConstraint();
ExplicitPtr_t enm = ert->createNumericalConstraint();
Configuration_t q = robot->currentConfiguration (),
qrand = se3::randomConfiguration(robot->model()),
......@@ -282,7 +282,7 @@ BOOST_AUTO_TEST_CASE (compare_to_relative_transform)
ExplicitRelativeTransformationPtr_t ert = ExplicitRelativeTransformation::create (
"explicit_relative_transformation", robot,
object1, object2, M1inO1, M2inO2);
constraints::ExplicitPtr_t enm = ert->createNumericalConstraint();
ExplicitPtr_t enm = ert->createNumericalConstraint();
DifferentiableFunctionPtr_t irt = enm->functionPtr();
RelativeTransformation::Ptr_t rt = RelativeTransformation::create (
......
tests/path-projectors.cc
View file @ 46604017
......@@ -61,6 +61,8 @@ using hpp::pinocchio::Device;
using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointPtr_t;
using hpp::constraints::Implicit;
using namespace hpp::core;
using namespace hpp::pinocchio;
......@@ -343,7 +345,7 @@ BOOST_AUTO_TEST_CASE_TEMPLATE (projectors, traits, test_types)
ConstraintSetPtr_t c = createConstraints (dev);
DifferentiableFunctionPtr_t func = traits::func (dev);
c->configProjector ()->add (constraints::Implicit::create (func));
c->configProjector ()->add (Implicit::create (func));
problem.steeringMethod(traits::SM_t::create (problem));
problem.steeringMethod ()->constraints (c);
......
tests/relative-motion.cc
View file @ 46604017
......@@ -30,7 +30,7 @@
#include <hpp/core/config-projector.hh>
#include <hpp/core/relative-motion.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/core/locked-joint.hh>
#include <hpp/constraints/locked-joint.hh>
#include <hpp/constraints/implicit.hh>
#include <hpp/core/explicit-relative-transformation.hh>
#include <pinocchio/multibody/joint/joint-variant.hpp>
......@@ -41,6 +41,7 @@ using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointPtr_t;
using hpp::constraints::RelativeTransformation;
using hpp::constraints::Implicit;
using namespace hpp::core;
using namespace hpp::pinocchio;
......@@ -186,7 +187,7 @@ BOOST_AUTO_TEST_CASE (relativeMotion)
dev->computeForwardKinematics ();
Transform3f tf1 (ja1->currentTransformation ());
Transform3f tf2 (jb2->currentTransformation ());
proj->add (constraints::Implicit::create (
proj->add (Implicit::create (
RelativeTransformation::create ("", dev, ja1, jb2, tf1, tf2)));
m = RelativeMotion::matrix(dev);
......
tests/roadmap-1.cc
View file @ 46604017
......@@ -26,7 +26,6 @@
#inclu