wangdequan/KDL_WORK
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

提交依赖与构建产物

Browse Source
This commit is contained in:
wangdequan
2026-06-27 09:25:04 -04:00
parent 2817cba164
commit f0e96308d2
1300 changed files with 844236 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

125
kdl_install/include/kdl/articulatedbodyinertia.hpp Normal file
View File

@@ -0,0 +1,125 @@
// Copyright (C) 2009 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_ARTICULATEDBODYINERTIA_HPP
#define KDL_ARTICULATEDBODYINERTIA_HPP
#include "frames.hpp"
#include "rotationalinertia.hpp"
#include "rigidbodyinertia.hpp"
#include <Eigen/Core>
namespace KDL {
/**
* \brief 6D Inertia of a articulated body
*
* The inertia is defined in a certain reference point and a certain reference base.
* The reference point does not have to coincide with the origin of the reference frame.
*/
class ArticulatedBodyInertia{
public:
/**
* This constructor creates a zero articulated body inertia matrix,
*/
ArticulatedBodyInertia(){
*this=ArticulatedBodyInertia::Zero();
}
/**
* This constructor creates a cartesian space articulated body inertia matrix,
* the arguments is a rigid body inertia.
*/
ArticulatedBodyInertia(const RigidBodyInertia& rbi);
/**
* This constructor creates a cartesian space inertia matrix,
* the arguments are the mass, the vector from the reference point to cog and the rotational inertia in the cog.
*/
explicit ArticulatedBodyInertia(double m, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero());
/**
* Creates an inertia with zero mass, and zero RotationalInertia
*/
static inline ArticulatedBodyInertia Zero(){
return ArticulatedBodyInertia(Eigen::Matrix3d::Zero(),Eigen::Matrix3d::Zero(),Eigen::Matrix3d::Zero());
};
~ArticulatedBodyInertia(){};
friend ArticulatedBodyInertia operator*(double a,const ArticulatedBodyInertia& I);
friend ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib);
friend ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib);
friend ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib);
friend ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib);
friend Wrench operator*(const ArticulatedBodyInertia& I,const Twist& t);
friend ArticulatedBodyInertia operator*(const Frame& T,const ArticulatedBodyInertia& I);
friend ArticulatedBodyInertia operator*(const Rotation& R,const ArticulatedBodyInertia& I);
/**
* Reference point change with v the vector from the old to
* the new point expressed in the current reference frame
*/
ArticulatedBodyInertia RefPoint(const Vector& p);
ArticulatedBodyInertia(const Eigen::Matrix3d& M,const Eigen::Matrix3d& H,const Eigen::Matrix3d& I);
Eigen::Matrix3d M;
Eigen::Matrix3d H;
Eigen::Matrix3d I;
};
/**
* Scalar product: I_new = double * I_old
*/
ArticulatedBodyInertia operator*(double a,const ArticulatedBodyInertia& I);
/**
* addition I: I_new = I_old1 + I_old2, make sure that I_old1
* and I_old2 are expressed in the same reference frame/point,
* otherwise the result is worth nothing
*/
ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib);
ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib);
ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib);
ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib);
/**
* calculate spatial momentum: h = I*v
* make sure that the twist v and the inertia are expressed in the same reference frame/point
*/
Wrench operator*(const ArticulatedBodyInertia& I,const Twist& t);
/**
* Coordinate system transform Ia = T_a_b*Ib with T_a_b the frame from a to b.
*/
ArticulatedBodyInertia operator*(const Frame& T,const ArticulatedBodyInertia& I);
/**
* Reference frame orientation change Ia = R_a_b*Ib with R_a_b
* the rotation of b expressed in a
*/
ArticulatedBodyInertia operator*(const Rotation& R,const ArticulatedBodyInertia& I);
}
#endif

105
kdl_install/include/kdl/chain.hpp Normal file
View File

@@ -0,0 +1,105 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAIN_HPP
#define KDL_CHAIN_HPP
#include "segment.hpp"
#include <string>
namespace KDL {
/**
* \brief This class encapsulates a <strong>serial</strong> kinematic
* interconnection structure. It is built out of segments.
*
* @ingroup KinematicFamily
*/
class Chain {
private:
unsigned int nrOfJoints;
unsigned int nrOfSegments;
public:
std::vector<Segment> segments;
/**
* The constructor of a chain, a new chain is always empty.
*
*/
Chain();
Chain(const Chain& in);
Chain& operator = (const Chain& arg);
/**
* Adds a new segment to the <strong>end</strong> of the chain.
*
* @param segment The segment to add
*/
void addSegment(const Segment& segment);
/**
* Adds a complete chain to the <strong>end</strong> of the chain
* The added chain is copied.
*
* @param chain The chain to add
*/
void addChain(const Chain& chain);
/**
* Request the total number of joints in the chain.\n
* <strong> Important:</strong> It is not the
* same as the total number of segments since a segment does not
* need to have a joint. This function is important when
* creating a KDL::JntArray to use with this chain.
* @return total nr of joints
*/
unsigned int getNrOfJoints()const {return nrOfJoints;};
/**
* Request the total number of segments in the chain.
* @return total number of segments
*/
unsigned int getNrOfSegments()const {return nrOfSegments;};
/**
* Request the nr'd segment of the chain. There is no boundary
* checking.
*
* @param nr the nr of the segment starting from 0
*
* @return a constant reference to the nr'd segment
*/
const Segment& getSegment(unsigned int nr)const;
/**
* Request the nr'd segment of the chain. There is no boundary
* checking.
*
* @param nr the nr of the segment starting from 0
*
* @return a reference to the nr'd segment
*/
Segment& getSegment(unsigned int nr);
virtual ~Chain();
};
}//end of namespace KDL
#endif

82
kdl_install/include/kdl/chaindynparam.hpp Normal file
View File

@@ -0,0 +1,82 @@
// Copyright (C) 2009 Dominick Vanthienen <dominick dot vanthienen at intermodalics dot eu>
// Version: 1.0
// Author: Dominick Vanthienen <dominick dot vanthienen at intermodalics dot eu>
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDLCHAINDYNPARAM_HPP
#define KDLCHAINDYNPARAM_HPP
#include "chainidsolver_recursive_newton_euler.hpp"
#include "articulatedbodyinertia.hpp"
#include "jntspaceinertiamatrix.hpp"
#include <Eigen/StdVector>
namespace KDL {
/**
* Implementation of a method to calculate the matrices H (inertia),C(coriolis) and G(gravitation)
* for the calculation torques out of the pose and derivatives.
* (inverse dynamics)
*
* The algorithm implementation for H is based on the book "Rigid Body
* Dynamics Algorithms" of Roy Featherstone, 2008
* (ISBN:978-0-387-74314-1) See page 107 for the pseudo-code.
* This algorithm is extended for the use of fixed joints
*
* It calculates the joint-space inertia matrix, given the motion of
* the joints (q,qdot,qdotdot), external forces on the segments
* (expressed in the segments reference frame) and the dynamical
* parameters of the segments.
*/
class ChainDynParam : public SolverI
{
public:
ChainDynParam(const Chain& chain, Vector _grav);
virtual ~ChainDynParam();
virtual int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis);
virtual int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H);
virtual int JntToGravity(const JntArray &q,JntArray &gravity);
/// @copydoc KDL::SolverI::updateInternalDataStructures()
virtual void updateInternalDataStructures();
private:
const Chain& chain;
int nr; // unused, remove in a future version
unsigned int nj;
unsigned int ns;
Vector grav;
Vector vectornull;
JntArray jntarraynull;
ChainIdSolver_RNE chainidsolver_coriolis;
ChainIdSolver_RNE chainidsolver_gravity;
std::vector<Wrench> wrenchnull;
std::vector<Frame> X;
std::vector<Twist> S;
//std::vector<RigidBodyInertia> I;
std::vector<ArticulatedBodyInertia, Eigen::aligned_allocator<ArticulatedBodyInertia> > Ic;
Wrench F;
Twist ag;
};
}
#endif

128
kdl_install/include/kdl/chainexternalwrenchestimator.hpp Normal file
View File

@@ -0,0 +1,128 @@
// Copyright (C) 2021 Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
// Version: 1.0
// Author: Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP
#define KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP
#include <Eigen/Core>
#include "utilities/svd_eigen_HH.hpp"
#include "chaindynparam.hpp"
#include "chainjnttojacsolver.hpp"
#include "chainfksolverpos_recursive.hpp"
#include <iostream>
namespace KDL {
/**
* \brief First-order momentum observer for the estimation of external wrenches applied on the robot's end-effector.
*
* Implementation based on:
* S. Haddadin, A. De Luca and A. Albu-Schäffer,
* "Robot Collisions: A Survey on Detection, Isolation, and Identification,"
* in IEEE Transactions on Robotics, vol. 33(6), pp. 1292-1312, 2017.
*
* Note: This component assumes that the external wrench is applied on the end-effector (last) link of the robot's chain.
*/
class ChainExternalWrenchEstimator : public SolverI
{
typedef Eigen::Matrix<double, 6, 1 > Vector6d;
public:
static const int E_FKSOLVERPOS_FAILED = -100; //! Internally-used Forward Position Kinematics (Recursive) solver failed
static const int E_JACSOLVER_FAILED = -101; //! Internally-used Jacobian solver failed
static const int E_DYNPARAMSOLVERMASS_FAILED = -102; //! Internally-used Dynamics Parameters (Mass) solver failed
static const int E_DYNPARAMSOLVERCORIOLIS_FAILED = -103; //! Internally-used Dynamics Parameters (Coriolis) solver failed
static const int E_DYNPARAMSOLVERGRAVITY_FAILED = -104; //! Internally-used Dynamics Parameters (Gravity) solver failed
/**
* Constructor for the estimator, it will allocate all the necessary memory
* \param chain The kinematic chain of the robot, an internal copy will be made.
* \param gravity The gravity-acceleration vector to use during the calculation.
* \param sample_frequency Frequency at which users updates it estimation loop (in Hz).
* \param estimation_gain Parameter used to control the estimator's convergence
* \param filter_constant Parameter defining how much the estimated signal should be filtered by the low-pass filter.
* This input value should be between 0 and 1. Higher the number means more noise needs to be filtered-out.
* The filter can be turned off by setting this value to 0.
* \param eps If a SVD-singular value is below this value, its inverse is set to zero. Default: 0.00001
* \param maxiter Maximum iterations for the SVD computations. Default: 150.
*/
ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant, const double eps = 0.00001, const int maxiter = 150);
~ChainExternalWrenchEstimator(){};
/**
* Calculates robot's initial momentum in the joint space.
* Basically, sets the offset for future estimation (momentum calculation).
* If this method is not called by the user, zero values will be taken for the initial momentum.
*/
int setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity);
// Sets singular-value eps parameter for the SVD calculation
void setSVDEps(const double eps_in);
// Sets maximum iteration parameter for the SVD calculation
void setSVDMaxIter(const int maxiter_in);
/**
* This method calculates the external wrench that is applied on the robot's end-effector.
* Input parameters:
* \param joint_position The current (measured) joint positions.
* \param joint_velocity The current (measured) joint velocities.
* \param joint_torque The joint space torques.
* Depending on the user's choice, this array can represent commanded or measured joint torques.
* A particular choice depends on the available sensors in robot's joint.
* For more details see the above-referenced article.
*
* Output parameters:
* \param external_wrench The estimated external wrench applied on the robot's end-effector.
* The wrench will be expressed w.r.t. end-effector's frame.
*
* @return error/success code
*/
int JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench);
// Returns the torques felt in the robot's joints as a result of the external wrench being applied on the robot.
void getEstimatedJntTorque(JntArray &external_joint_torque);
/// @copydoc KDL::SolverI::updateInternalDataStructures()
virtual void updateInternalDataStructures();
/// @copydoc KDL::SolverI::strError()
virtual const char* strError(const int error) const;
private:
const Chain &CHAIN;
const double DT_SEC, FILTER_CONST;
double svd_eps;
int svd_maxiter;
unsigned int nj, ns;
JntSpaceInertiaMatrix jnt_mass_matrix, previous_jnt_mass_matrix, jnt_mass_matrix_dot;
JntArray initial_jnt_momentum, estimated_momentum_integral, filtered_estimated_ext_torque,
gravity_torque, coriolis_torque, total_torque, estimated_ext_torque;
Jacobian jacobian_end_eff;
Eigen::MatrixXd jacobian_end_eff_transpose, jacobian_end_eff_transpose_inv, U, V;
Eigen::VectorXd S, S_inv, tmp, ESTIMATION_GAIN;
ChainDynParam dynparam_solver;
ChainJntToJacSolver jacobian_solver;
ChainFkSolverPos_recursive fk_pos_solver;
};
}
#endif

62
kdl_install/include/kdl/chainfdsolver.hpp Normal file
View File

@@ -0,0 +1,62 @@
// Copyright (C) 2018 Ruben Smits <ruben dot smits at intermodalics dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at intermodalics dot be>
// Author: Craig Carignan <craigc at ssl dot umd dot edu>
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAIN_FDSOLVER_HPP
#define KDL_CHAIN_FDSOLVER_HPP
#include "chain.hpp"
#include "frames.hpp"
#include "jntarray.hpp"
#include "solveri.hpp"
namespace KDL
{
typedef std::vector<Wrench> Wrenches;
/**
* \brief This <strong>abstract</strong> class encapsulates the inverse
* dynamics solver for a KDL::Chain.
*
*/
class ChainFdSolver : public KDL::SolverI
{
public:
/**
* Calculate forward dynamics from joint positions, joint velocities, joint torques/forces,
* and externally applied forces/torques to joint accelerations.
*
* @param q input joint positions
* @param q_dot input joint velocities
* @param torque input joint torques
* @param f_ext external forces
*
* @param q_dotdot output joint accelerations
*
* @return if < 0 something went wrong
*/
virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext,JntArray &q_dotdot)=0;
};
}
#endif

111
kdl_install/include/kdl/chainfdsolver_recursive_newton_euler.hpp Normal file
View File

@@ -0,0 +1,111 @@
// Copyright (C) 2018 Craig Carignan <craigc at ssl dot umd dot edu>
// Version: 1.0
// Author: Craig Carignan <craigc at ssl dot umd dot edu>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAIN_FDSOLVER_RECURSIVE_NEWTON_EULER_HPP
#define KDL_CHAIN_FDSOLVER_RECURSIVE_NEWTON_EULER_HPP
#include "chainfdsolver.hpp"
#include "chainidsolver_recursive_newton_euler.hpp"
#include "chaindynparam.hpp"
namespace KDL{
/**
* \brief Recursive newton euler forward dynamics solver
*
* The algorithm implementation is based on the book "Rigid Body
* Dynamics Algorithms" of Roy Featherstone, 2008
* (ISBN:978-0-387-74314-1) See Chapter 6 for basic algorithm.
*
* It calculates the accelerations for the joints (qdotdot), given the
* position and velocity of the joints (q,qdot,qdotdot), external forces
* on the segments (expressed in the segments reference frame),
* and the dynamical parameters of the segments.
*/
class ChainFdSolver_RNE : public ChainFdSolver{
public:
/**
* Constructor for the solver, it will allocate all the necessary memory
* \param chain The kinematic chain to calculate the forward dynamics for, an internal copy will be made.
* \param grav The gravity vector to use during the calculation.
*/
ChainFdSolver_RNE(const Chain& chain, Vector grav);
~ChainFdSolver_RNE(){};
/**
* Function to calculate from Cartesian forces to joint torques.
* Input parameters;
* \param q The current joint positions
* \param q_dot The current joint velocities
* \param torques The current joint torques (applied by controller)
* \param f_ext The external forces (no gravity) on the segments
* Output parameters:
* \param q_dotdot The resulting joint accelerations
*/
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &torques, const Wrenches& f_ext, JntArray &q_dotdot);
/// @copydoc KDL::SolverI::updateInternalDataStructures
virtual void updateInternalDataStructures();
/**
* Function to integrate the joint accelerations resulting from the forward dynamics solver.
* Input parameters;
* \param nj The number of joints
* \param t The current time
* \param dt The integration period
* \param q The current joint positions
* \param q_dot The current joint velocities
* \param torques The current joint torques (applied by controller)
* \param f_ext The external forces (no gravity) on the segments
* \param fdsolver The forward dynamics solver
* Output parameters:
* \param t The updated time
* \param q The updated joint positions
* \param q_dot The updated joint velocities
* \param q_dotdot The current joint accelerations
* \param dq The joint position increment
* \param dq_dot The joint velocity increment
* Temporary parameters:
* \param qtemp Intermediate joint positions
* \param qdtemp Intermediate joint velocities
*/
void RK4Integrator(unsigned int& nj, const double& t, double& dt, KDL::JntArray& q, KDL::JntArray& q_dot,
KDL::JntArray& torques, KDL::Wrenches& f_ext, KDL::ChainFdSolver_RNE& fdsolver,
KDL::JntArray& q_dotdot, KDL::JntArray& dq, KDL::JntArray& dq_dot,
KDL::JntArray& q_temp, KDL::JntArray& q_dot_temp);
private:
const Chain& chain;
ChainDynParam DynSolver;
ChainIdSolver_RNE IdSolver;
unsigned int nj;
unsigned int ns;
JntSpaceInertiaMatrix H;
JntArray Tzeroacc;
Eigen::MatrixXd H_eig;
Eigen::VectorXd Tzeroacc_eig;
Eigen::MatrixXd L_eig;
Eigen::VectorXd D_eig;
Eigen::VectorXd r_eig;
Eigen::VectorXd acc_eig;
};
}
#endif

141
kdl_install/include/kdl/chainfksolver.hpp Normal file
View File

@@ -0,0 +1,141 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAIN_FKSOLVER_HPP
#define KDL_CHAIN_FKSOLVER_HPP
#include "chain.hpp"
#include "framevel.hpp"
#include "frameacc.hpp"
#include "jntarray.hpp"
#include "jntarrayvel.hpp"
#include "jntarrayacc.hpp"
#include "solveri.hpp"
namespace KDL {
/**
* \brief This <strong>abstract</strong> class encapsulates a
* solver for the forward position kinematics for a KDL::Chain.
*
* @ingroup KinematicFamily
*/
//Forward definition
class ChainFkSolverPos : public KDL::SolverI {
public:
/**
* Calculate forward position kinematics for a KDL::Chain,
* from joint coordinates to cartesian pose.
*
* @param q_in input joint coordinates
* @param p_out reference to output cartesian pose
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1)=0;
/**
* Calculate forward position kinematics for a KDL::Chain,
* from joint coordinates to cartesian pose.
*
* @param q_in input joint coordinates
* @param p_out reference to a vector of output cartesian poses for all segments
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArray& q_in, std::vector<KDL::Frame>& p_out,int segmentNr=-1)=0;
virtual void updateInternalDataStructures()=0;
virtual ~ChainFkSolverPos(){};
};
/**
* \brief This <strong>abstract</strong> class encapsulates a solver
* for the forward velocity kinematics for a KDL::Chain.
*
* @ingroup KinematicFamily
*/
class ChainFkSolverVel : public KDL::SolverI {
public:
/**
* Calculate forward position and velocity kinematics, from
* joint coordinates to cartesian coordinates.
*
* @param q_in input joint coordinates (position and velocity)
* @param out output cartesian coordinates (position and velocity)
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0;
/**
* Calculate forward position and velocity kinematics, from
* joint coordinates to cartesian coordinates.
*
* @param q_in input joint coordinates (position and velocity)
* @param out output cartesian coordinates for all segments (position and velocity)
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArrayVel& q_in, std::vector<KDL::FrameVel>& out,int segmentNr=-1)=0;
virtual void updateInternalDataStructures()=0;
virtual ~ChainFkSolverVel(){};
};
/**
* \brief This <strong>abstract</strong> class encapsulates a solver
* for the forward acceleration kinematics for a KDL::Chain.
*
* @ingroup KinematicFamily
*/
class ChainFkSolverAcc : public KDL::SolverI {
public:
/**
* Calculate forward position, velocity and acceleration
* kinematics, from joint coordinates to cartesian coordinates
*
* @param q_in input joint coordinates (position, velocity and
* acceleration
@param out output cartesian coordinates (position, velocity
* and acceleration
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0;
/**
* Calculate forward position, velocity and acceleration
* kinematics, from joint coordinates to cartesian coordinates
*
* @param q_in input joint coordinates (position, velocity and
* acceleration
* @param out output cartesian coordinates (position, velocity
* and acceleration for all segments
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArrayAcc& q_in, std::vector<FrameAcc>& out,int segmentNr=-1)=0;
virtual void updateInternalDataStructures()=0;
virtual ~ChainFkSolverAcc()=0;
};
}//end of namespace KDL
#endif

53
kdl_install/include/kdl/chainfksolverpos_recursive.hpp Normal file
View File

@@ -0,0 +1,53 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDLCHAINFKSOLVERPOS_RECURSIVE_HPP
#define KDLCHAINFKSOLVERPOS_RECURSIVE_HPP
#include "chainfksolver.hpp"
namespace KDL {
/**
* Implementation of a recursive forward position kinematics
* algorithm to calculate the position transformation from joint
* space to Cartesian space of a general kinematic chain (KDL::Chain).
*
* @ingroup KinematicFamily
*/
class ChainFkSolverPos_recursive : public ChainFkSolverPos
{
public:
ChainFkSolverPos_recursive(const Chain& chain);
~ChainFkSolverPos_recursive();
virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1);
virtual int JntToCart(const JntArray& q_in, std::vector<Frame>& p_out, int segmentNr=-1);
virtual void updateInternalDataStructures() {};
private:
const Chain& chain;
};
}
#endif

51
kdl_install/include/kdl/chainfksolvervel_recursive.hpp Normal file
View File

@@ -0,0 +1,51 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAIN_FKSOLVERVEL_RECURSIVE_HPP
#define KDL_CHAIN_FKSOLVERVEL_RECURSIVE_HPP
#include "chainfksolver.hpp"
namespace KDL
{
/**
* Implementation of a recursive forward position and velocity
* kinematics algorithm to calculate the position and velocity
* transformation from joint space to Cartesian space of a general
* kinematic chain (KDL::Chain).
*
* @ingroup KinematicFamily
*/
class ChainFkSolverVel_recursive : public ChainFkSolverVel
{
public:
ChainFkSolverVel_recursive(const Chain& chain);
~ChainFkSolverVel_recursive();
virtual int JntToCart(const JntArrayVel& q_in,FrameVel& out,int segmentNr=-1);
virtual int JntToCart(const JntArrayVel& q_in,std::vector<FrameVel>& out,int segmentNr=-1);
virtual void updateInternalDataStructures() {};
private:
const Chain& chain;
};
}
#endif

535
kdl_install/include/kdl/chainhdsolver_vereshchagin.hpp Normal file
View File

@@ -0,0 +1,535 @@
// Copyright (C) 2009 Ruben Smits <ruben dot smits at intermodalics dot eu>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
// Author: Herman Bruyninckx
// Author: Azamat Shakhimardanov
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP
#define KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP
#include "chainidsolver.hpp"
#include "frames.hpp"
#include "articulatedbodyinertia.hpp"
#include<Eigen/StdVector>
namespace KDL
{
/**
* \brief **Abstract**: Acceleration constrained hybrid dynamics calculations for a chain, based on Vereshchagin 1989.
* This class creates an instance of the hybrid dynamics solver. The solver analytically calculates the joint space
* constraint torques and acceleration in a chain when a constraint force(s) is applied to the chain's end-effector
* (task space / cartesian space). In the robotics literature, this algorithm is also known under the following names:
* Acceleration Constrained Hybrid Dynamics (ACHD) and Popov-Vereshchagin solver.
*
* ## INTRODUCTION
*
* In 1970', researchers [1], [2] have developed a hybrid dynamics algorithm for evaluating robot behavior
* based on the input specification that is defined by the Cartesian acceleration constraints,
* feed-forward joint torques and external Cartesian wrenches. The solver is derived from a well-known
* principle of mechanics - **Gauss' principle of least constraint** [6] and provides an analytical (closed-form)
* solution to the hybrid dynamics problem with linear-time, **O(n)** complexity [3].
*
* In general, the Gauss' principle states that the true motion (acceleration) of a system/body is defined
* by the minimum of a quadratic function that is subject to linear geometric motion constraints [7], [6].
* The result of this Gauss function represents the **acceleration energy** of a body, which is defined by
* the product of its mass and the squared distance between its allowed (constrained) acceleration and its free
* (unconstrained) acceleration [10], [11]. In the case of originally derived Popov-Vereshchagin algorithm [1],
* geometric motion constraints are Cartesian acceleration constraints imposed on the robot's end-effector.
* This domain-specific solver minimizes the acceleration energy by performing computational (outward and inward)
* sweeps along the robot's kinematic chain [3]. Furthermore, by computing the minimum of Gauss function,
* the Popov-Vereshchagin solver resolves the kinematic redundancy of the robot, when a partial motion (task)
* specification is provided [2]. A necessary condition that enables this type of closed-form algorithm
* (an analytical solution to the above-described optimization problem) defines that the robot’s kinematic
* chain does not consist of closed loops, i.e. the robot’s kinematic chain must be constructed in a serial
* or tree structure [11]. However, it is always possible to cut these loops and introduce explicit constrains.
*
* For evaluating robot dynamics, i.e. resolving its constrained motion, the Popov-Vereshchagin solver is
* performing three computational sweeps (recursions), along the kinematic chain [3]. More specifically,
* two sweeps in **outward** and one sweep in **inward** direction. In the case of robot dynamics algorithms,
* the outward sweep refers to a recursion that is covering a kinematic chain from proximal to distal segments,
* while the inward sweep is covering a kinematic chain from distal to proximal segments [5]. Additionally,
* after completing the recursion in the second sweep and before starting the recursion in the last sweep,
* the solver is computing magnitudes of constraint forces, i.e. the Langrage multiplier (noted as **nu** in
* the KDL's solver implementation and original solver's publication[2]). More specifically, this operation is
* performed when the algorithm reaches segment (link) **{0}**, namely the base segment. In this formulation
* of the solver, the gravity effects are taken into account by setting the base-link's acceleration equal
* to gravitational acceleration [3].
*
* For more detailed description of the algorithm and its representation, the reader can refer to [3], [5], [11].
*
* ## INTERFACES
*
* ### Solver's Input
*
* For computing solutions to the constrained hybrid dynamics problem, this original derivation of
* the Popov-Vereshchagin solver [3] takes into account the following inputs:
*
* * Robot's **model** defined by: kinematic parameters of the chain, segments' mass and rigid-body
* inertia, and effective inertia of each joint rotor -> **chain** parameter in solver's constructor
*
* * **Root** acceleration of the robot's base segment (usually gravitational) -> **root_acc** parameter
* in solver's constructor
*
* * Current joint configuration (angles) -> **q** parameter in the **CartToJnt** function
*
* * Current joint velocities -> **q_dot** parameter in the **CartToJnt** function
*
* * Motion drivers:
* * - **Cartesian Acceleration Constraints** imposed on the end-effector segment -> **alpha** and
* **beta** parameters in the **CartToJnt** function
* * - **Cartesian External Wrench** acting on each segment -> **f_ext** parameter in the **CartToJnt** function
* * - **Feed-Forward Torque** acting on each joint -> **ff_torques** parameter in the **CartToJnt** function
*
* The following outlines the above-listed task interfaces in more detail.
*
* #### Cartesian Acceleration Constraints: alpha & beta
*
* This first type of motion driver can be used for specifying **physical** constraints such as contacts with environment [3],
* or **artificial** (i.e. task-imposed) constraints defined by the operational space task definition for the end-effector
* (tool-tip) segment. **Note**: the Vereshchagin solver expects that the input Cartesian Acceleration Constraints, i.e.
* unit constraint forces in alpha parameters, are expressed w.r.t. robot's base frame. However, the acceleration energy
* setpoints, i.e. beta parameters, are expressed w.r.t. above-defined unit constraint forces. More specifically, each DOF
* (element) in beta parameter corresponds to its respective DOF (column) of the unit constraint force matrix (alpha) [11].
*
* To use this interface, a user should define **i)** the active constraint directions via **alpha** parameter, which is
* a **6 x m** matrix of spatial unit constraint forces, and **ii)** acceleration energy setpoints via **beta**, which is
* a **m x 1** vector. Here, the number of constraints **m**, or in another words number of spatial unit constraint forces
* is not required to always be equal to **6**, which means that a human programmer can leave some of the degrees of freedom
* unspecified [2] for this motion driver, and still produce valid joint control commands [5]. For example, if we want to
* constrain the motion of the end-effector segment in only one direction, namely linear **x**-direction, we can define
* the constraint as [3]:
*
* **alpha** =
* | |
* | --|
* | 1 |
* | 0 |
* | 0 |
* | 0 |
* | 0 |
* | 0 |
*
* **beta** = | 0 |
*
* Note that here, the first three rows of matrix **alpha** represent linear elements and the last three rows represent
* angular elements, of the spatial unit force defined in Plücker coordinates [4]. By giving zero value to acceleration
* energy setpoint (**beta**), we are defining that the end-effector is not allowed to have linear acceleration in **x**
* direction. Or in other words, we are restricting the robot from producing any acceleration energy in that specified direction.
*
* Another example includes the specification of constraints in **5 DOFs**. We can constrain the motion of robot's end-effector
* such that it is only allowed to **freely** move in the linear **z**-direction, without performing linear motions in **x**
* and **y** and angular motions in **x**, **y** and **z** directions:
*
* **alpha** =
* | | | | | |
* | --| --| --| --| --|
* | 1 | 0 | 0 | 0 | 0 |
* | 0 | 1 | 0 | 0 | 0 |
* | 0 | 0 | 0 | 0 | 0 |
* | 0 | 0 | 1 | 0 | 0 |
* | 0 | 0 | 0 | 1 | 0 |
* | 0 | 0 | 0 | 0 | 1 |
*
* **beta** =
* | |
* | --|
* | 0 |
* | 0 |
* | 0 |
* | 0 |
* | 0 |
*
* For both above-described task examples, the Acceleration Constrained Hybrid Dynamics (ACHD) solver will compute
* valid control (constraint) joint torques, even though some of the Cartesian DOFs are left unspecified
* (e.g. in the case of the second example that would be end-effector's **z**-direction). More specifically:
* ``Underconstrained motion specifications are naturally resolved using Gauss' principle of least constraint`` [5].
* This means that in those directions in which the robot is not constrained by the task definition, its motions will
* be controlled by the nature. For instance, in the second example, natural resolution of the robot motion would define
* that the end-effector "**falls**" in the linear **z** direction due to effects of gravity, with the assumption that
* gravity forces are acting along $z$-direction.
*
* Moreover, the motion specification in the second example is equivalent to:
*
* **alpha** =
* | | | | | | |
* | --| --| --| --| --| --|
* | 1 | 0 | 0 | 0 | 0 | 0 |
* | 0 | 1 | 0 | 0 | 0 | 0 |
* | 0 | 0 | 0 | 0 | 0 | 0 |
* | 0 | 0 | 0 | 1 | 0 | 0 |
* | 0 | 0 | 0 | 0 | 1 | 0 |
* | 0 | 0 | 0 | 0 | 0 | 1 |
* (note that elements in the third column are all zeros, meaning z-linear constraint is deactivated)
*
* **beta** =
* | |
* | --|
* | 0 |
* | 0 |
* | 0 |
* | 0 |
* | 0 |
* | 0 |
*
* The last example involves the full specification of the desired end-effector motion (in this case,
* not necessarily zero accelerations), i.e. specification of constraints in all 6 **DOFs**:
*
* **alpha** =
* | | | | | | |
* | --| --| --| --| --| --|
* | 1 | 0 | 0 | 0 | 0 | 0 |
* | 0 | 1 | 0 | 0 | 0 | 0 |
* | 0 | 0 | 1 | 0 | 0 | 0 |
* | 0 | 0 | 0 | 1 | 0 | 0 |
* | 0 | 0 | 0 | 0 | 1 | 0 |
* | 0 | 0 | 0 | 0 | 0 | 1 |
*
* **beta** = **alpha^T * X_dotdot_N**
*
* Here, **N** stands for the index of the last robot's segment, end-effector (tool-tip). The reader should note
* that we can directly assign values (magnitudes) of the desired (task-defined) spatial acceleration **6 x 1**
* vector **X_dotdot_N** to the **6 x 1** vector of acceleration energy (**beta**) [3]. Even though physical
* dimensions (units) of these two vectors are not the same, the property of matrix **alpha** (it contains
* **unit** vectors), permits that we can assign values of desired accelerations to acceleration energy setpoints,
* in respective directions. Namely, each column of matrix **alpha** has the value of **1** in the respective
* direction in which constraint force works, thus it follows that the value of acceleration energy setpoint is
* the same as the value of Cartesian acceleration, in the respective direction.
*
* #### External Forces: f_ext
*
* This type of driver can be used for specifying **physical** (but not artificial, i.e. not task-introduced)
* Cartesian wrenches acting on each of the robot's segments [11]. Examples for a **physical** force on a segment can be:
* **i)** a known weight at the robot's gripper, for instance, a grasped cup or **ii)** a force from a human pushing
* the robot [5]. Note that the implementation of Vereshchagin solver in KDL expects the provided **f_ext** is
* expressed w.r.t. robot's base frame, which is in contrast to the case of KDL's RNE solver.
*
* ### Feed-Forward Joint Torques: ff_torque
*
* This type of motion driver can be used for specifying **physical** (but not artificial, i.e. not task-introduced)
* joint torques, for example, spring and/or damper-based torques (e.g. friction effects) in robot's joints [11].
*
* Additional examples on using these input interfaces can be found in "../tests/solvertest.cpp":
*
* * VereshchaginTest() function - an example on how to use all interfaces of this solver for computing
* the solution to the Hybrid Dynamics (HD) problem.
*
* * FdAndVereshchaginSolversConsistencyTest() function - an example on how to only use this solver for
* computing the solution to the Featherstone's (i.e. Articulated Body Algorithm (ABA)) version of
* the Forward Dynamics (FD) problem.
*
* ### Solver's Output
*
* This recursive dynamics solver is computing several quantities that represent solutions to both, inverse and
* forward dynamics problems, or in other words solutions to the constrained hybrid dynamics problem.
* More specifically, the output interface of the original Popov-Vereshchagin algorithm consists of [3], [5]:
*
* * Magnitudes of constraint forces that act on the end-effector, denoted by the Lagrange multiplier **nu**
* in the solver's implementation and original solver's publication[2].
*
* * Joint constraint torques required for achieving the desired (acceleration-constraints-defined) behavior of
* the robot: **constraint_torque**. These torques represent control commands that should be sent to robot's joint drivers.
*
* * Argument that defines the solution to the originally formulated optimization problem in **Gauss' principle**.
* In other words, the joint accelerations **q_dotdot** resulting from the total torque acting on each joint (**total_torque**),
* i.e. from the aforementioned constraint torques and all natural and external forces acting on the system.
*
* * The resulting and complete spatial accelerations of each segment in the kinematic chain: **X_dotdot**
*
* Furthermore, if necessary, a complete spatial vector of imposed constraint forces can be computed [3],
* from the following relation: **alpha * nu**.
*
* The reader should note that this **constraint_torque** is the **necessary** control command that a user is supposed
* to send to robot's joints, to achieve the motion that is computed (resolved) by the Popov-Vereshchagin solver.
* More specifically, here **constraint_torque** represent solution to the **Inverse Dynamics (ID)** problem.
* Nevertheless, the reason why a user is not supposed to use the **total_torque** values as the control commands
* for robot's joints, is the fact that the torque contributions that represent the difference between **total_torque**
* and **constraint_torque** already exist (act) on robot joints. More specifically, these **additional (residual)**
* contributions are produced on the joints by the already existing natural forces that act on the system [11].
*
* On the other side, joint accelerations, namely **q_dotdot** provide solution to the **Forward Dynamics (FD)** problem
* and these quantities can be used for both control (integrate to joint positions/velocities) and simulation purposes [11].
*
* ## PRACTICAL INSIGHTS/CONSIDERATIONS
*
* The Popov-Vereshchagin hybrid dynamics solver enables a user to achieve many types of operational space tasks [11].
* In other words, various controllers can be implemented **around** the aforementioned interfaces of the algorithm.
* Examples can be controllers for hybrid force/position control, impedance control, etc. However, there are some practical
* insights about this algorithm that need to taken into account.
*
* ### Prioritizations between motion drivers (interfaces)
*
* The original derivation of this solver, which is considered in this library, prioritizes Cartesian acceleration constrains
* (specified for the end-effector segment) over other two motion drivers (Cartesian external wrenches and feedforward joint torques) [11].
* In practice, this means the following:
*
* * If the external wrenches and/or feedforward joint torques contribute positively (i.e. assist) in producing Cartesian accelerations
* (specified via acceleration constraint interface), the Vereshchagin solver will take advantage of these forces to compute
* (acceleration-) energy optimal motions.
*
* * On the other hand, if the aforementioned external wrenches and/or feedforward joint torques contribute negatively (i.e. interfere)
* the Cartesian accelerations, the Vereshchagin solver will compensate all of those forces to correctly produce constrained
* accelerations of the end-effector. More specifically, additional torque commands will be computed under
* **constrained joint torques** (**ctrl_torques** in this implementation), to overcome those "disturbances".
*
* Nevertheless, the above-described prioritization can be changed (see [3] & [5] for more details) but those features are not implemented in KDL.
*
* ### Using the algorithm for solving forward dynamics (FD) problem
*
* The reader should note that the Popov-Vereshchagin solver represents an extension to the well-known forward-dynamics
* Articulated Body Algorithm (ABA) developed by Featherstone and described in [4] (moreover, Featherstone mentioned Vereshchagin solver
* in his book [4], page 117). This means that the Popov-Vereshchagin solver can also be purely used as this Articulated Body Algorithm
* forward-dynamics algorithm. In that case, it is necessary for the user to deactivate all Cartesian acceleration constraints (it is sufficient
* to set all elements in **alpha** matrix to zero) and proceed using other two interfaces as in the case of standard FD solver. More specifically,
* use **f_ext** input to define **physical** external wrenches acting on the robot's body (should be expressed w.r.t. robot's base frame) and
* **ff_torque** input to define command torques acting in robot's joints. The resulting robot's motion can be taken from **q_dotdot** and
* **X_dotdot** solver's outputs.
*
* Nevertheless, the Popov-Vereshchagin solver can also be used for solving more advanced forward dynamics problems, than those solved by ABA [4].
* More specifically, if this solver is used in a certain simulation environment for the use-case of simulating robot behaviors, all three
* interfaces can be exploited for defining a more descriptive robot's state. Here, a user can exploit the Cartesian acceleration constraint
* interface to specify different constraints imposed on the end-effector, along with other interfaces, and simulate what would be the robot's
* behavior due to these constraints and environmental impacts. Here, the resulting robot's motion can, as well, be taken from **q_dotdot** and
* **X_dotdot** solver's outputs. For example, MuJoCo framework (see MuJoCo's documentation) also uses Gauss' principle of least constraint to
* simulate constraint forces in certain situations, however, there, final derivation of this principle in the software is different. However, in
* the case of this more advanced forward dynamics computations, the user needs to be aware of prioritizations between input interfaces
* (mentioned in "Prioritizations" section above) and internal policies on
* handling singularities (mentioned in "Singularities and matrix inversions" section below).
*
* ### Singularities and matrix inversions
*
* To find the minimal-energy solution to the Inverse Dynamics (ID) problem, i.e. find the Langrage multiplier **nu**, the solver needs to compute
* the inverse of a so-called "acceleration constraint coupling matrix"[3] in the balance equation before starting the third sweep.
* However, the robot's configuration has a direct impact on this matrix and its inversion. Namely, if the robot is in a singular configuration for
* the task specified via acceleration constraints, this matrix will become rank-deficient. This means that it is not possible to find a feasible
* solution for that particular end-effector's DOF that is (or DOFs that are) lost due to the singular configuration. In other words, it is not
* possible to find **constraint torques** that will satisfy imposed acceleration constraints in that DOF/DOFs. Nevertheless, in this situation,
* it is still possible to find the (energy-optimal) solution for other "non-singular" DOFs. For that reason, in KDL's implementation of the solver,
* matrix inverse is found by using the SVD technique to construct a pseudo inverse. Additionally, in this implementation, a control policy is
* introduced via the truncated-SVD method to deactivate, i.e. more specifically **ignore**, acceleration constraints for the DOFs that are lost due
* to robot's singular configuration. Of course, this is a choice (control policy), i.e. only one option for solving the singularity problem and
* producing safe joint commands. It is left for the user to explore other control policies (options) for this particular problem if of course,
* the user is not satisfied with the current control policy.
*
* ### Supported robot models
*
* KDL's current implementation of the Vereshchagin HD solver supports only robot chains that have equal number of joints and segments.
* Moreover, this implementation can only compute dynamics for **serial** type of chains, i.e. currently, **tree** robot structures are not supported
* in this solver. Nevertheless, the original solver's derivation has been extended in [3] to account for multiple motion constraints imposed
* on a **tree** robot structure. This extension does not only account for acceleration constraints imposed on multiple end-effectors but also for
* acceleration constraints imposed on more proximal segments. However, the above-mentioned extensions are currently not implemented in this version of KDL.
*
* ## REFERENCES
*
* [1] E. P. Popov, A. F. Vereshchagin, and S. L. Zenkevich, "Manipulyatsionnye roboty: Dinamika i algoritmy", Nauka, Moscow, 1978.
*
* [2] A. F. Vereshchagin, “Modelling and control of motion of manipulation robots”, Soviet Journal of Computer and Systems Sciences, vol. 27, pp. 29–38, 1989.
*
* [3] A. Shakhimardanov, “Composable robot motion stack: Implementing constrained hybrid dynamics using semantic models of kinematic chains”, PhD thesis, KU Leuven, 2015.
*
* [4] R. Featherstone, Rigid body dynamics algorithms. Springer, 2008.
*
* [5] S. Schneider and H. Bruyninckx, “Exploiting linearity in dynamics solvers for the design of composable robotic manipulation architectures”, in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2019.
*
* [6] H. Bruyninckx and O. Khatib, "Gauss’ principle and the dynamics of redundant and constrained manipulators", in IEEE International Conference on Robotics and Automation, 2000.
*
* [7] C. F. Gauß, "Über ein neues allgemeines Grundgesetz der Mechanik.", Journal für die reine und angewandte Mathematik, vol. 4, pp. 232–235, 1829.
*
* [8] A. F. Vereshchagin, “Computer simulation of the dynamics of complicated mechanisms of robot-manipulators”, Engineering Cybernetics, 12(6), pp. 65–70, 1974.
*
* [9] E. P. Popov, "Control of robots-manipulators", Engineering Cybernetics, 1974.
*
* [10] E. Ramm, “Principles of least action and of least constraint”, GAMM-Mitteilungen, vol. 34, pp. 164–182, 2011.
*
* [11] D. Vukcevic, "Lazy Robot Control by Relaxation of Motion and Force Constraints." Technical Report/Hochschule Bonn-Rhein-Sieg University of Applied Sciences, Department of Computer Science, 2020.
*
* @ingroup KinematicFamily
*/
class ChainHdSolver_Vereshchagin : KDL::SolverI
{
typedef std::vector<Twist> Twists;
typedef std::vector<Frame> Frames;
typedef Eigen::Matrix<double, 6, 1 > Vector6d;
typedef Eigen::Matrix<double, 6, 6 > Matrix6d;
typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd;
public:
/**
* Constructor for the solver, it will allocate all the necessary memory
* \param chain The kinematic chain to calculate the hybrid dynamics for. An internal copy will be made.
* \param root_acc The acceleration twist of the root segment to use during the calculation (usually contains gravity).
* Note: This solver takes gravity acceleration with opposite sign comparead to the KDL's FD and RNE solvers
* \param nc Number of constraints imposed on the robot's end-effector (maximum is 6).
*/
ChainHdSolver_Vereshchagin(const Chain& chain, const Twist &root_acc, const unsigned int nc);
~ChainHdSolver_Vereshchagin()
{
};
/**
* This method calculates joint space constraint torques and accelerations.
* It returns 0 when it succeeds, otherwise -1 or -2 for nonmatching matrix and array sizes.
* Input parameters:
* \param q The current joint positions
* \param q_dot The current joint velocities
* \param alpha The active constraint directions (unit constraint forces expressed w.r.t. robot's base frame)
* \param beta The acceleration energy setpoints (expressed w.r.t. above-defined unit constraint forces)
* \param f_ext The external forces (no gravity, it is given in root acceleration) on the segments
* \param ff_torques The feed-forward joint space torques
*
* Output parameters:
* \param q_dotdot The resulting joint accelerations
* \param constraint_torques The resulting joint constraint torques (what each joint feels due to the constraint forces acting on the end-effector)
*
* @return error/success code
*/
int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, const JntArray &ff_torques, JntArray &constraint_torques);
/// @copydoc KDL::SolverI::updateInternalDataStructures
virtual void updateInternalDataStructures();
//Returns cartesian acceleration of links in base coordinates
void getTransformedLinkAcceleration(Twists& x_dotdot);
// Returns total torque acting on each joint (constraints + nature + external forces)
void getTotalTorque(JntArray &total_tau);
// Returns magnitude of the constraint forces acting on the end-effector: Lagrange Multiplier
void getContraintForceMagnitude(Eigen::VectorXd &nu_);
/*
//Returns cartesian positions of links in base coordinates
void getLinkCartesianPose(Frames& x_base);
//Returns cartesian velocities of links in base coordinates
void getLinkCartesianVelocity(Twists& xDot_base);
//Returns cartesian acceleration of links in base coordinates
void getLinkCartesianAcceleration(Twists& xDotDot_base);
//Returns cartesian positions of links in link tip coordinates
void getLinkPose(Frames& x_local);
//Returns cartesian velocities of links in link tip coordinates
void getLinkVelocity(Twists& xDot_local);
//Returns cartesian acceleration of links in link tip coordinates
void getLinkAcceleration(Twists& xDotdot_local);
//Acceleration energy due to unit constraint forces at the end-effector
void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M);
//Acceleration energy due to arm configuration: bias force plus input joint torques
void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G);
void getLinkUnitForceMatrix(Matrix6Xd& E_tilde);
void getLinkBiasForceMatrix(Wrenches& R_tilde);
void getJointBiasAcceleration(JntArray &bias_q_dotdot);
*/
private:
/**
* This method calculates all cartesian space poses, twists, bias accelerations.
* External forces are also taken into account in this outward sweep.
*/
void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext);
/**
* This method is a force balance sweep. It calculates articulated body inertias and bias forces.
* Additionally, acceleration energies generated by bias forces and unit forces are calculated here.
*/
void downwards_sweep(const Jacobian& alfa, const JntArray& ff_torques);
/**
* This method calculates constraint force magnitudes.
*
*/
void constraint_calculation(const JntArray& beta);
/**
* This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together.
*
*/
void final_upwards_sweep(JntArray &q_dotdot, JntArray &constraint_torques);
private:
const Chain& chain;
unsigned int nj;
unsigned int ns;
unsigned int nc;
Twist acc_root;
Jacobian alfa_N;
Jacobian alfa_N2;
Eigen::MatrixXd M_0_inverse;
Eigen::MatrixXd Um;
Eigen::MatrixXd Vm;
JntArray beta_N;
Eigen::VectorXd nu;
Eigen::VectorXd nu_sum;
Eigen::VectorXd Sm;
Eigen::VectorXd tmpm;
Eigen::VectorXd total_torques; // all the contributions that are felt at the joint: constraints + nature + external forces
Wrench qdotdot_sum;
Frame F_total;
struct segment_info
{
Frame F; //local pose with respect to previous link in segments coordinates
Frame F_base; // pose of a segment in root coordinates
Twist Z; //Unit twist
Twist v; //twist
Twist acc; //acceleration twist
Wrench U; //wrench p of the bias forces (in cartesian space)
Wrench R; //wrench p of the bias forces
Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form
Twist C; //constraint
Twist A; //constraint
ArticulatedBodyInertia H; //I (expressed in 6*6 matrix)
ArticulatedBodyInertia P; //I (expressed in 6*6 matrix)
ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix)
Wrench PZ; //vector U[i] = I_A[i]*S[i]
Wrench PC; //vector E[i] = I_A[i]*c[i]
double D; //vector D[i] = S[i]^T*U[i]
Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints
Matrix6Xd E_tilde;
Eigen::MatrixXd M; //acceleration energy already generated at link i
Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i
Eigen::VectorXd EZ; //K[i] = Etiltde'*Z
double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration
double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration
double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration
double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace
double u; //vector u[i] = torques(i) - S[i]^T*(p_A[i] + I_A[i]*C[i]) in joint subspace. Azamat: In code u[i] = torques(i) - s[i].totalBias
segment_info(unsigned int nc):
D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0)
{
E.resize(6, nc);
E_tilde.resize(6, nc);
G.resize(nc);
M.resize(nc, nc);
EZ.resize(nc);
E.setZero();
E_tilde.setZero();
M.setZero();
G.setZero();
EZ.setZero();
};
};
std::vector<segment_info, Eigen::aligned_allocator<segment_info> > results;
};
}
#endif // KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP

62
kdl_install/include/kdl/chainidsolver.hpp Normal file
View File

@@ -0,0 +1,62 @@
// Copyright (C) 2009 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAIN_IDSOLVER_HPP
#define KDL_CHAIN_IDSOLVER_HPP
#include "chain.hpp"
#include "frames.hpp"
#include "jntarray.hpp"
#include "solveri.hpp"
namespace KDL
{
typedef std::vector<Wrench> Wrenches;
/**
* \brief This <strong>abstract</strong> class encapsulates the inverse
* dynamics solver for a KDL::Chain.
*
*/
class ChainIdSolver : public KDL::SolverI
{
public:
/**
* Calculate inverse dynamics, from joint positions, velocity, acceleration, external forces
* to joint torques/forces.
*
* @param q input joint positions
* @param q_dot input joint velocities
* @param q_dotdot input joint accelerations
*
* @param torque output joint torques
*
* @return if < 0 something went wrong
*/
virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques)=0;
// Need functions to return the manipulator mass, coriolis and gravity matrices - Lagrangian Formulation.
};
}
#endif

78
kdl_install/include/kdl/chainidsolver_recursive_newton_euler.hpp Normal file
View File

@@ -0,0 +1,78 @@
// Copyright (C) 2009 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAIN_IKSOLVER_RECURSIVE_NEWTON_EULER_HPP
#define KDL_CHAIN_IKSOLVER_RECURSIVE_NEWTON_EULER_HPP
#include "chainidsolver.hpp"
namespace KDL{
/**
* \brief Recursive newton euler inverse dynamics solver
*
* The algorithm implementation is based on the book "Rigid Body
* Dynamics Algorithms" of Roy Featherstone, 2008
* (ISBN:978-0-387-74314-1) See page 96 for the pseudo-code.
*
* It calculates the torques for the joints, given the motion of
* the joints (q,qdot,qdotdot), external forces on the segments
* (expressed in the segments reference frame) and the dynamical
* parameters of the segments.
*/
class ChainIdSolver_RNE : public ChainIdSolver{
public:
/**
* Constructor for the solver, it will allocate all the necessary memory
* \param chain The kinematic chain to calculate the inverse dynamics for, an internal copy will be made.
* \param grav The gravity vector to use during the calculation.
*/
ChainIdSolver_RNE(const Chain& chain,Vector grav);
~ChainIdSolver_RNE(){};
/**
* Function to calculate from Cartesian forces to joint torques.
* Input parameters;
* \param q The current joint positions
* \param q_dot The current joint velocities
* \param q_dotdot The current joint accelerations
* \param f_ext The external forces (no gravity) on the segments
* Output parameters:
* \param torques the resulting torques for the joints
*/
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques);
/// @copydoc KDL::SolverI::updateInternalDataStructures
virtual void updateInternalDataStructures();
private:
const Chain& chain;
unsigned int nj;
unsigned int ns;
std::vector<Frame> X;
std::vector<Twist> S;
std::vector<Twist> v;
std::vector<Twist> a;
std::vector<Wrench> f;
Twist ag;
};
}
#endif

40
kdl_install/include/kdl/chainidsolver_vereshchagin.hpp Normal file
View File

@@ -0,0 +1,40 @@
// Copyright (C) 2009 Ruben Smits <ruben dot smits at intermodalics dot eu>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
// Author: Herman Bruyninckx
// Author: Azamat Shakhimardanov
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP
#define KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP
#include "chainhdsolver_vereshchagin.hpp"
namespace KDL
{
class ChainIdSolver_Vereshchagin : public ChainHdSolver_Vereshchagin
{
public:
ChainIdSolver_Vereshchagin(const Chain& chain, const Twist &root_acc, const unsigned int nc);
};
}
#endif // KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP

169
kdl_install/include/kdl/chainiksolver.hpp Normal file
View File

@@ -0,0 +1,169 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAIN_IKSOLVER_HPP
#define KDL_CHAIN_IKSOLVER_HPP
#include "chain.hpp"
#include "frames.hpp"
#include "framevel.hpp"
#include "frameacc.hpp"
#include "jntarray.hpp"
#include "jntarrayvel.hpp"
#include "jntarrayacc.hpp"
#include "solveri.hpp"
namespace KDL {
/**
* \brief This <strong>abstract</strong> class encapsulates the inverse
* position solver for a KDL::Chain.
*
* @ingroup KinematicFamily
*/
class ChainIkSolverPos : public KDL::SolverI {
public:
/**
* Calculate inverse position kinematics, from cartesian
*coordinates to joint coordinates.
*
* @param q_init initial guess of the joint coordinates
* @param p_in input cartesian coordinates
* @param q_out output joint coordinates
*
* @return if < 0 something went wrong
*/
virtual int CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)=0;
virtual ~ChainIkSolverPos(){};
virtual void updateInternalDataStructures()=0;
};
/**
* \brief This <strong>abstract</strong> class encapsulates the inverse
* velocity solver for a KDL::Chain.
*
* @ingroup KinematicFamily
*/
class ChainIkSolverVel : public KDL::SolverI {
public:
/**
* Calculate inverse velocity kinematics, from joint positions
*and cartesian velocity to joint velocities.
*
* @param q_in input joint positions
* @param v_in input cartesian velocity
* @param qdot_out output joint velocities
*
* @return if < 0 something went wrong
*/
virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)=0;
/**
* Calculate inverse position and velocity kinematics, from
*cartesian position and velocity to joint positions and velocities.
*
* @param q_init initial joint positions
* @param v_in input cartesian position and velocity
* @param q_out output joint position and velocity
*
* @return if < 0 something went wrong
*/
virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out)=0;
virtual ~ChainIkSolverVel(){};
virtual void updateInternalDataStructures()=0;
};
/**
* \brief This <strong>abstract</strong> class encapsulates the inverse
* acceleration solver for a KDL::Chain.
*
* @ingroup KinematicFamily
*/
class ChainIkSolverAcc : public KDL::SolverI {
public:
/**
* Calculate inverse acceleration kinematics from joint
* positions, joint velocities and cartesian acceleration to joint accelerations.
*
* @param q_in input joint positions
* @param qdot_in input joint velocities
* @param a_in input cartesian acceleration
* @param qdotdot_out output joint accelerations
*
* @return if < 0 something went wrong
*/
virtual int CartToJnt(const JntArray& q_in, const JntArray& qdot_in, const Twist a_in,
JntArray& qdotdot_out)=0;
/**
* Calculate inverse position, velocity and acceration
*kinematics from cartesian coordinates to joint coordinates
*
* @param q_init initial guess for joint positions
* @param a_in input cartesian position, velocity and acceleration
* @param q_out output joint position, velocity and acceleration
*
* @return if < 0 something went wrong
*/
virtual int CartTojnt(const JntArray& q_init, const FrameAcc& a_in,
JntArrayAcc& q_out)=0;
/**
* Calculate inverse velocity and acceleration kinematics from
* joint positions and cartesian velocity and acceleration to
* joint velocities and accelerations.
*
* @param q_in input joint positions
* @param v_in input cartesian velocity
* @param a_in input cartesian acceleration
* @param qdot_out output joint velocities
* @param qdotdot_out output joint accelerations
*
* @return if < 0 something went wrong
*/
virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, const Twist& a_in,
JntArray& qdot_out, JntArray& qdotdot_out)=0;
/**
* Calculate inverse position and acceleration kinematics from
*joint velocities and cartesian position and acceleration to
*joint positions and accelerations
*
* @param q_init initial guess for joint positions
* @param p_in input cartesian position
* @param qdot_in input joint velocities
* @param a_in input cartesian acceleration
* @param q_out output joint positions
* @param qdotdot_out output joint accelerations
*
* @return if < 0 something went wrong
*/
virtual int CartTojnt(const JntArray& q_init, const Frame& p_in, const JntArray& qdot_in, const Twist& a_in,
JntArray& q_out, JntArray& qdotdot_out)=0;
virtual void updateInternalDataStructures()=0;
virtual ~ChainIkSolverAcc(){};
};
}//end of namespace KDL
#endif

256
kdl_install/include/kdl/chainiksolverpos_lma.hpp Normal file
View File

@@ -0,0 +1,256 @@
#ifndef KDL_CHAINIKSOLVERPOS_GN_HPP
#define KDL_CHAINIKSOLVERPOS_GN_HPP
/**
\file chainiksolverpos_lma.hpp
\brief computing inverse position kinematics using Levenberg-Marquardt.
*/
/**************************************************************************
begin : May 2012
copyright : (C) 2012 Erwin Aertbelien
email : firstname.lastname@mech.kuleuven.ac.be
History (only major changes)( AUTHOR-Description ) :
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
#include "chainiksolver.hpp"
#include "chain.hpp"
#include <Eigen/Dense>
namespace KDL
{
/**
* \brief Solver for the inverse position kinematics that uses Levenberg-Marquardt.
*
* The robustness and speed of this solver is improved in several ways:
* - by using a Levenberg-Marquardt method that automatically adapts the damping when
* computing the inverse damped least squares inverse velocity kinematics.
* - by using an internal implementation of forward position kinematics and the
* Jacobian kinematics. This implementation is more numerically robust,
* is able to cache previous computations, and implements an \f$ \mathcal{O}(N) \f$
* algorithm for the computation of the Jacobian (with \f$N\f$, the number of joints, and for
* a fixed size task space).
* - by providing a way to specify the weights in task space, you can weigh rotations wrt translations.
* This is important e.g. to specify that rotations do not matter for the problem at hand, or to
* specify how important you judge rotations w.r.t. translations, typically in S.I.-units, ([m],[rad]),
* the rotations are over-specified, this can be avoided using the weight matrix. <B>Weights also
* make the solver more robust </B>.
* - only the constructors call <B>memory allocation</B>.
*
* De general principles behind the optimisation is inspired on:
* Jorge Nocedal, Stephen J. Wright, Numerical Optimization,Springer-Verlag New York, 1999.
* \ingroup KinematicFamily
*/
class ChainIkSolverPos_LMA : public KDL::ChainIkSolverPos
{
private:
typedef double ScalarType;
typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,Eigen::Dynamic> MatrixXq;
typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,1> VectorXq;
public:
static const int E_GRADIENT_JOINTS_TOO_SMALL = -100;
static const int E_INCREMENT_JOINTS_TOO_SMALL = -101;
/**
* \brief constructs an ChainIkSolverPos_LMA solver.
*
* The default parameters are chosen to be applicable to industrial-size robots
* (e.g. 0.5 to 3 meters range in task space), with an accuracy that is more then
* sufficient for typical industrial applications.
*
* Weights are applied in task space, i.e. the kinematic solver minimizes:
* \f$ E = \Delta \mathbf{x}^T \mathbf{L} \mathbf{L}^T \Delta \mathbf{x} \f$, with \f$\mathbf{L}\f$ a diagonal matrix.
*
* \param _chain specifies the kinematic chain.
* \param _l specifies the "square root" of the weight (diagonal) matrix in task space. This diagonal matrix is specified as a vector.
* \param _eps specifies the desired accuracy in task space; <B>after</B> weighing with
* the weight matrix, it is applied on \f$E\f$.
* \param _maxiter specifies the maximum number of iterations.
* \param _eps_joints specifies that the algorithm has to stop when the computed joint angle increments are
* smaller then _eps_joints. This is to avoid unnecessary computations up to _maxiter when the joint angle
* increments are so small that they effectively (in floating point) do not change the joint angles any more. The default
* is a few digits above numerical accuracy.
*/
ChainIkSolverPos_LMA(
const KDL::Chain& _chain,
const Eigen::Matrix<double,6,1>& _l,
double _eps=1E-5,
int _maxiter=500,
double _eps_joints=1E-15
);
/**
* \brief identical the full constructor for ChainIkSolverPos_LMA, but provides for a default weight matrix.
*
* \f$\mathbf{L} = \mathrm{diag}\left( \begin{bmatrix} 1 & 1 & 1 & 0.01 & 0.01 & 0.01 \end{bmatrix} \right) \f$.
*/
ChainIkSolverPos_LMA(
const KDL::Chain& _chain,
double _eps=1E-5,
int _maxiter=500,
double _eps_joints=1E-15
);
/**
* \brief computes the inverse position kinematics.
*
* \param q_init initial joint position.
* \param T_base_goal goal position expressed with respect to the robot base.
* \param q_out joint position that achieves the specified goal position (if successful).
* \return E_NOERROR if successful,
* E_GRADIENT_JOINTS_TOO_SMALL the gradient of \f$ E \f$ towards the joints is to small,
* E_INCREMENT_JOINTS_TOO_SMALL if joint position increments are to small,
* E_MAX_ITER_EXCEEDED if number of iterations is exceeded.
*/
virtual int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out);
/**
* \brief destructor.
*/
virtual ~ChainIkSolverPos_LMA();
/**
* \brief for internal use only.
*
* Only exposed for test and diagnostic purposes.
*/
void compute_fwdpos(const VectorXq& q);
/**
* \brief for internal use only.
* Only exposed for test and diagnostic purposes.
* compute_fwdpos(q) should always have been called before.
*/
void compute_jacobian(const VectorXq& q);
/**
* \brief for internal use only.
* Only exposed for test and diagnostic purposes.
*/
void display_jac(const KDL::JntArray& jval);
/// @copydoc KDL::SolverI::updateInternalDataStructures
void updateInternalDataStructures();
/// @copydoc KDL::SolverI::strError()
virtual const char* strError(const int error) const;
private:
const KDL::Chain& chain;
unsigned int nj;
unsigned int ns;
public:
/**
* \brief contains the last number of iterations for an execution of CartToJnt.
*/
int lastNrOfIter;
/**
* \brief contains the last value for \f$ E \f$ after an execution of CartToJnt.
*/
double lastDifference;
/**
* \brief contains the last value for the (unweighted) translational difference after an execution of CartToJnt.
*/
double lastTransDiff;
/**
* \brief contains the last value for the (unweighted) rotational difference after an execution of CartToJnt.
*/
double lastRotDiff;
/**
* \brief contains the last values for the singular values of the weighted Jacobian after an execution of CartToJnt.
*/
VectorXq lastSV;
/**
* \brief for internal use only.
*
* contains the last value for the Jacobian after an execution of compute_jacobian.
*/
MatrixXq jac;
/**
* \brief for internal use only.
*
* contains the gradient of the error criterion after an execution of CartToJnt.
*/
VectorXq grad;
/**
* \brief for internal use only.
*
* contains the last value for the position of the tip of the robot (head) with respect to the base, after an execution of compute_jacobian.
*/
KDL::Frame T_base_head;
/**
* \brief display information on each iteration step to the console.
*/
bool display_information;
private:
// additional specification of the inverse position kinematics problem:
unsigned int maxiter;
double eps;
double eps_joints;
Eigen::Matrix<ScalarType,6,1> L;
// state of compute_fwdpos and compute_jacobian:
std::vector<KDL::Frame> T_base_jointroot;
std::vector<KDL::Frame> T_base_jointtip;
// need 2 vectors because of the somewhat strange definition of segment.hpp
// you could also recompute jointtip out of jointroot,
// but then you'll need more expensive cos/sin functions.
// the following are state of CartToJnt that is pre-allocated:
VectorXq q;
MatrixXq A;
VectorXq tmp;
Eigen::LDLT<MatrixXq> ldlt;
Eigen::JacobiSVD<MatrixXq> svd;
VectorXq diffq;
VectorXq q_new;
VectorXq original_Aii;
};
} // namespace KDL
#endif

97
kdl_install/include/kdl/chainiksolverpos_nr.hpp Normal file
View File

@@ -0,0 +1,97 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDLCHAINIKSOLVERPOS_NR_HPP
#define KDLCHAINIKSOLVERPOS_NR_HPP
#include "chainiksolver.hpp"
#include "chainfksolver.hpp"
namespace KDL {
/**
* Implementation of a general inverse position kinematics
* algorithm based on Newton-Raphson iterations to calculate the
* position transformation from Cartesian to joint space of a general
* KDL::Chain.
*
* @ingroup KinematicFamily
*/
class ChainIkSolverPos_NR : public ChainIkSolverPos
{
public:
static const int E_IKSOLVER_FAILED = -100; //! Child IK solver vel failed
static const int E_FKSOLVERPOS_FAILED = -101; //! Child FK solver failed
/**
* Constructor of the solver, it needs the chain, a forward
* position kinematics solver and an inverse velocity
* kinematics solver for that chain.
*
* @param chain the chain to calculate the inverse position for
* @param fksolver a forward position kinematics solver
* @param iksolver an inverse velocity kinematics solver
* @param maxiter the maximum Newton-Raphson iterations,
* default: 100
* @param eps the precision for the position, used to end the
* iterations, default: epsilon (defined in kdl.hpp)
*
* @return
*/
ChainIkSolverPos_NR(const Chain& chain,ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver,
unsigned int maxiter=100,double eps=1e-6);
~ChainIkSolverPos_NR();
/**
* Find an output joint pose \a q_out, given a starting joint pose
* \a q_init and a desired cartesian pose \a p_in
*
* @return:
* E_NOERROR=solution converged to <eps in maxiter
* E_DEGRADED=solution converged to <eps in maxiter, but solution is
* degraded in quality (e.g. pseudo-inverse in iksolver is singular)
* E_IKSOLVER_FAILED=velocity solver failed
* E_NO_CONVERGE=solution did not converge (e.g. large displacement, low iterations)
*/
virtual int CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out);
/// @copydoc KDL::SolverI::strError()
virtual const char* strError(const int error) const;
/// @copydoc KDL::SolverI::updateInternalDataStructures
virtual void updateInternalDataStructures();
private:
const Chain& chain;
unsigned int nj;
ChainIkSolverVel& iksolver;
ChainFkSolverPos& fksolver;
JntArray delta_q;
Frame f;
Twist delta_twist;
unsigned int maxiter;
double eps;
};
}
#endif

129
kdl_install/include/kdl/chainiksolverpos_nr_jl.hpp Normal file
View File

@@ -0,0 +1,129 @@
// Copyright (C) 2007-2008 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Copyright (C) 2008 Mikael Mayer
// Copyright (C) 2008 Julia Jesse
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDLCHAINIKSOLVERPOS_NR_JL_HPP
#define KDLCHAINIKSOLVERPOS_NR_JL_HPP
#include "chainiksolver.hpp"
#include "chainfksolver.hpp"
namespace KDL {
/**
* Implementation of a general inverse position kinematics
* algorithm based on Newton-Raphson iterations to calculate the
* position transformation from Cartesian to joint space of a general
* KDL::Chain. Takes joint limits into account.
*
* @ingroup KinematicFamily
*/
class ChainIkSolverPos_NR_JL : public ChainIkSolverPos
{
public:
static const int E_IKSOLVERVEL_FAILED = -100; //! Child IK solver vel failed
static const int E_FKSOLVERPOS_FAILED = -101; //! Child FK solver failed
/**
* Constructor of the solver, it needs the chain, a forward
* position kinematics solver and an inverse velocity
* kinematics solver for that chain.
*
* @param chain the chain to calculate the inverse position for
* @param q_min the minimum joint positions
* @param q_max the maximum joint positions
* @param fksolver a forward position kinematics solver
* @param iksolver an inverse velocity kinematics solver
* @param maxiter the maximum Newton-Raphson iterations,
* default: 100
* @param eps the precision for the position, used to end the
* iterations, default: epsilon (defined in kdl.hpp)
*
* @return
*/
ChainIkSolverPos_NR_JL(const Chain& chain,const JntArray& q_min, const JntArray& q_max, ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver,unsigned int maxiter=100,double eps=1e-6);
/**
* Constructor of the solver, it needs the chain, a forward
* position kinematics solver and an inverse velocity
* kinematics solver for that chain.
*
* @param chain the chain to calculate the inverse position for
* @param fksolver a forward position kinematics solver
* @param iksolver an inverse velocity kinematics solver
* @param maxiter the maximum Newton-Raphson iterations,
* default: 100
* @param eps the precision for the position, used to end the
* iterations, default: epsilon (defined in kdl.hpp)
*
* @return
*/
ChainIkSolverPos_NR_JL(const Chain& chain, ChainFkSolverPos& fksolver,ChainIkSolverVel& iksolver,unsigned int maxiter=100,double eps=1e-6);
~ChainIkSolverPos_NR_JL();
/**
* Calculates the joint values that correspond to the input pose given an initial guess.
* @param q_init Initial guess for the joint values.
* @param p_in The input pose of the chain tip.
* @param q_out The resulting output joint values
* @return E_MAX_ITERATIONS_EXCEEDED if the maximum number of iterations was exceeded before a result was found
* E_NOT_UP_TO_DATE if the internal data is not up to date with the chain
* E_SIZE_MISMATCH if the size of the input/output data does not match the chain.
*/
virtual int CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out);
/**
* Function to set the joint limits.
* @param q_min minimum values for the joints
* @param q_max maximum values for the joints
* @return E_SIZE_MISMATCH if input sizes do not match the chain
*/
int setJointLimits(const JntArray& q_min, const JntArray& q_max);
/// @copydoc KDL::SolverI::updateInternalDataStructures
virtual void updateInternalDataStructures();
/// @copydoc KDL::SolverI::strError()
const char* strError(const int error) const;
private:
const Chain& chain;
unsigned int nj;
JntArray q_min;
JntArray q_max;
ChainIkSolverVel& iksolver;
ChainFkSolverPos& fksolver;
JntArray delta_q;
unsigned int maxiter;
double eps;
Frame f;
Twist delta_twist;
};
}
#endif

118
kdl_install/include/kdl/chainiksolvervel_pinv.hpp Normal file
View File

@@ -0,0 +1,118 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_HPP
#define KDL_CHAIN_IKSOLVERVEL_PINV_HPP
#include "chainiksolver.hpp"
#include "chainjnttojacsolver.hpp"
#include "utilities/svd_HH.hpp"
namespace KDL
{
/**
* Implementation of a inverse velocity kinematics algorithm based
* on the generalize pseudo inverse to calculate the velocity
* transformation from Cartesian to joint space of a general
* KDL::Chain. It uses a svd-calculation based on householders
* rotations.
*
* @ingroup KinematicFamily
*/
class ChainIkSolverVel_pinv : public ChainIkSolverVel
{
public:
/// solution converged but (pseudo)inverse is singular
static const int E_CONVERGE_PINV_SINGULAR = +100;
/**
* Constructor of the solver
*
* @param chain the chain to calculate the inverse velocity
* kinematics for
* @param eps if a singular value is below this value, its
* inverse is set to zero, default: 0.00001
* @param maxiter maximum iterations for the svd calculation,
* default: 150
*
*/
explicit ChainIkSolverVel_pinv(const Chain& chain,double eps=0.00001,int maxiter=150);
~ChainIkSolverVel_pinv();
/**
* Find an output joint velocity \a qdot_out, given a starting joint pose
* \a q_init and a desired cartesian velocity \a v_in
*
* @return
* E_NOERROR=solution converged to <eps in maxiter
* E_SVD_FAILED=SVD computation failed
* E_CONVERGE_PINV_SINGULAR=solution converged but (pseudo)inverse is singular
*
* @note if E_CONVERGE_PINV_SINGULAR returned then converged and can
* continue motion, but have degraded solution
*
* @note If E_SVD_FAILED returned, then getSvdResult() returns the error code
* from the SVD algorithm.
*/
virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
/**
* not (yet) implemented.
*
*/
virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return (error = E_NOT_IMPLEMENTED);};
/**
* Retrieve the number of singular values of the jacobian that are < eps;
* if the number of near zero singular values is > jac.col()-jac.row(),
* then the jacobian pseudoinverse is singular
*/
unsigned int getNrZeroSigmas()const {return nrZeroSigmas;};
/**
* Retrieve the latest return code from the SVD algorithm
* @return 0 if CartToJnt() not yet called, otherwise latest SVD result code.
*/
int getSVDResult()const {return svdResult;};
/// @copydoc KDL::SolverI::strError()
virtual const char* strError(const int error) const;
/// @copydoc KDL::SolverI::updateInternalDataStructures
virtual void updateInternalDataStructures();
private:
const Chain& chain;
ChainJntToJacSolver jnt2jac;
unsigned int nj;
Jacobian jac;
SVD_HH svd;
std::vector<JntArray> U;
JntArray S;
std::vector<JntArray> V;
JntArray tmp;
double eps;
int maxiter;
unsigned int nrZeroSigmas;
int svdResult;
};
}
#endif

57
kdl_install/include/kdl/chainiksolvervel_pinv_givens.hpp Normal file
View File

@@ -0,0 +1,57 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP
#define KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP
#include "chainiksolver.hpp"
#include "chainjnttojacsolver.hpp"
#include <Eigen/Core>
namespace KDL
{
/**
* Implementation of a inverse velocity kinematics algorithm based
* on the generalize pseudo inverse to calculate the velocity
* transformation from Cartesian to joint space of a general
* KDL::Chain. It uses a svd-calculation based on householders
* rotations.
*
* @ingroup KinematicFamily
*/
class ChainIkSolverVel_pinv_givens : public ChainIkSolverVel
{
public:
/**
* Constructor of the solver
*
* @param chain the chain to calculate the inverse velocity
* kinematics for
*
*/
explicit ChainIkSolverVel_pinv_givens(const Chain& chain);
~ChainIkSolverVel_pinv_givens();
virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
/**
* not (yet) implemented.
*
*/
virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return (error = E_NOT_IMPLEMENTED);};
/// @copydoc KDL::SolverI::updateInternalDataStructures
virtual void updateInternalDataStructures();
private:
const Chain& chain;
unsigned int nj;
ChainJntToJacSolver jnt2jac;
Jacobian jac;
bool transpose,toggle;
unsigned int m,n;
Eigen::MatrixXd jac_eigen,U,V,B;
Eigen::VectorXd S,tempi,UY,SUY,qdot_eigen,v_in_eigen;
};
}
#endif

160
kdl_install/include/kdl/chainiksolvervel_pinv_nso.hpp Normal file
View File

@@ -0,0 +1,160 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_NSO_HPP
#define KDL_CHAIN_IKSOLVERVEL_PINV_NSO_HPP
#include "chainiksolver.hpp"
#include "chainjnttojacsolver.hpp"
#include <Eigen/Core>
namespace KDL
{
/**
* Implementation of a inverse velocity kinematics algorithm based
* on the generalize pseudo inverse to calculate the velocity
* transformation from Cartesian to joint space of a general
* KDL::Chain. It uses a svd-calculation based on householders
* rotations.
*
* In case of a redundant robot this solver optimizes the following criterium:
* g=0.5*sum(weight*(Desired_joint_positions - actual_joint_positions))^2 as described in
* A. Liegeois. Automatic supervisory control of the configuration and
* behavior of multibody mechanisms. IEEE Transactions on Systems, Man, and
* Cybernetics, 7(12):868–871, 1977
*
* @ingroup KinematicFamily
*/
class ChainIkSolverVel_pinv_nso : public ChainIkSolverVel
{
public:
/**
* Constructor of the solver
*
* @param chain the chain to calculate the inverse velocity
* kinematics for
* @param opt_pos the desired positions of the chain used by to resolve the redundancy
* @param weights the weights applied in the joint space
* @param eps if a singular value is below this value, its
* inverse is set to zero, default: 0.00001
* @param maxiter maximum iterations for the svd calculation,
* default: 150
* @param alpha the null-space velocity gain
*
*/
ChainIkSolverVel_pinv_nso(const Chain& chain, const JntArray& opt_pos, const JntArray& weights, double eps=0.00001,int maxiter=150, double alpha = 0.25);
explicit ChainIkSolverVel_pinv_nso(const Chain& chain, double eps=0.00001,int maxiter=150, double alpha = 0.25);
~ChainIkSolverVel_pinv_nso();
virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
/**
* not (yet) implemented.
*
*/
virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return (error = E_NOT_IMPLEMENTED);};
/**
* Request the joint weights for optimization criterion
*
*
* @return const reference to the joint weights
*/
const JntArray& getWeights()const
{
return weights;
}
/**
* Request the optimal joint positions
*
*
* @return const reference to the optimal joint positions
*/
const JntArray& getOptPos()const
{
return opt_pos;
}
/**
* Request null space velocity gain
*
*
* @return const reference to the null space velocity gain
*/
const double& getAlpha()const
{
return alpha;
}
/**
*Set joint weights for optimization criterion
*
*@param weights the joint weights
*
*/
virtual int setWeights(const JntArray &weights);
/**
*Set optimal joint positions
*
*@param opt_pos optimal joint positions
*
*/
virtual int setOptPos(const JntArray &opt_pos);
/**
*Set null space velocity gain
*
*@param alpha NUllspace velocity cgain
*
*/
virtual int setAlpha(const double alpha);
/**
* Retrieve the latest return code from the SVD algorithm
* @return 0 if CartToJnt() not yet called, otherwise latest SVD result code.
*/
int getSVDResult()const {return svdResult;};
/// @copydoc KDL::SolverI::updateInternalDataStructures
virtual void updateInternalDataStructures();
private:
const Chain& chain;
ChainJntToJacSolver jnt2jac;
unsigned int nj;
Jacobian jac;
Eigen::MatrixXd U;
Eigen::VectorXd S;
Eigen::VectorXd Sinv;
Eigen::MatrixXd V;
Eigen::VectorXd tmp;
Eigen::VectorXd tmp2;
double eps;
int maxiter;
int svdResult;
double alpha;
JntArray weights;
JntArray opt_pos;
};
}
#endif

244
kdl_install/include/kdl/chainiksolvervel_wdls.hpp Normal file
View File

@@ -0,0 +1,244 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAIN_IKSOLVERVEL_WDLS_HPP
#define KDL_CHAIN_IKSOLVERVEL_WDLS_HPP
#include "chainiksolver.hpp"
#include "chainjnttojacsolver.hpp"
#include <Eigen/Core>
namespace KDL
{
/**
* Implementation of a inverse velocity kinematics algorithm based
* on the weighted pseudo inverse with damped least-square to calculate the velocity
* transformation from Cartesian to joint space of a general
* KDL::Chain. It uses a svd-calculation based on householders
* rotations.
*
* J# = M_q*Vb*pinv_dls(Db)*Ub'*M_x
*
* where B = Mx*J*Mq
*
* and B = Ub*Db*Vb' is the SVD decomposition of B
*
* Mq and Mx represent, respectively, the joint-space and task-space weighting
* matrices.
* Please refer to the documentation of setWeightJS(const Eigen::MatrixXd& Mq)
* and setWeightTS(const Eigen::MatrixXd& Mx) for details on the effects of
* these matrices.
*
* For more details on Weighted Pseudo Inverse, see :
* 1) [Ben Israel 03] A. Ben Israel & T.N.E. Greville.
* Generalized Inverses : Theory and Applications,
* second edition. Springer, 2003. ISBN 0-387-00293-6.
*
* 2) [Doty 93] K. L. Doty, C. Melchiorri & C. Boniveto.
* A theory of generalized inverses applied to Robotics.
* The International Journal of Robotics Research,
* vol. 12, no. 1, pages 1-19, february 1993.
*
*
* @ingroup KinematicFamily
*/
class ChainIkSolverVel_wdls : public ChainIkSolverVel
{
public:
/// solution converged but (pseudo)inverse is singular
static const int E_CONVERGE_PINV_SINGULAR = +100;
/**
* Constructor of the solver
*
* @param chain the chain to calculate the inverse velocity
* kinematics for
* @param eps if a singular value is below this value, its
* inverse is set to zero, default: 0.00001
* @param maxiter maximum iterations for the svd calculation,
* default: 150
*
*/
explicit ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=150);
//=ublas::identity_matrix<double>
~ChainIkSolverVel_wdls();
/**
* Find an output joint velocity \a qdot_out, given a starting joint pose
* \a q_init and a desired cartesian velocity \a v_in
*
* @return
* E_NOERROR=svd solution converged in maxiter
* E_SVD_FAILED=svd solution failed
* E_CONVERGE_PINV_SINGULAR=svd solution converged but (pseudo)inverse singular
*
* @note if E_CONVERGE_PINV_SINGULAR returned then converged and can
* continue motion, but have degraded solution
*
* @note If E_SVD_FAILED returned, then getSvdResult() returns the error
* code from the SVD algorithm.
*/
virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
/**
* not (yet) implemented.
*
*/
virtual int CartToJnt(const JntArray& /*q_init*/, const FrameVel& /*v_in*/, JntArrayVel& /*q_out*/){return -1;};
/**
* Set the joint space weighting matrix
*
* @param weight_js joint space weighting symmetric matrix,
* default : identity. M_q : This matrix being used as a
* weight for the norm of the joint space speed it HAS TO BE
* symmetric and positive definite. We can actually deal with
* matrices containing a symmetric and positive definite block
* and 0s otherwise. Taking a diagonal matrix as an example, a
* 0 on the diagonal means that the corresponding joints will
* not contribute to the motion of the system. On the other
* hand, the bigger the value, the most the corresponding
* joint will contribute to the overall motion. The obtained
* solution q_dot will actually minimize the weighted norm
* sqrt(q_dot'*(M_q^-2)*q_dot). In the special case we deal
* with, it does not make sense to invert M_q but what is
* important is the physical meaning of all this : a joint
* that has a zero weight in M_q will not contribute to the
* motion of the system and this is equivalent to saying that
* it gets an infinite weight in the norm computation. For
* more detailed explanation : vincent.padois@upmc.fr
*
* @return success/error code
*/
int setWeightJS(const Eigen::MatrixXd& Mq);
/**
* Set the task space weighting matrix
*
* @param weight_ts task space weighting symmetric matrix,
* default: identity M_x : This matrix being used as a weight
* for the norm of the error (in terms of task space speed) it
* HAS TO BE symmetric and positive definite. We can actually
* deal with matrices containing a symmetric and positive
* definite block and 0s otherwise. Taking a diagonal matrix
* as an example, a 0 on the diagonal means that the
* corresponding task coordinate will not be taken into
* account (ie the corresponding error can be really big). If
* the rank of the jacobian is equal to the number of task
* space coordinates which do not have a 0 weight in M_x, the
* weighting will actually not impact the results (ie there is
* an exact solution to the velocity inverse kinematics
* problem). In cases without an exact solution, the bigger
* the value, the most the corresponding task coordinate will
* be taken into account (ie the more the corresponding error
* will be reduced). The obtained solution will minimize the
* weighted norm sqrt(|x_dot-Jq_dot|'*(M_x^2)*|x_dot-Jq_dot|).
* For more detailed explanation : vincent.padois@upmc.fr
*
* @return success/error code
*/
int setWeightTS(const Eigen::MatrixXd& Mx);
/**
* Set lambda
*/
void setLambda(const double lambda);
/**
* Set eps
*/
void setEps(const double eps_in);
/**
* Set maxIter
*/
void setMaxIter(const int maxiter_in);
/**
* Request the number of singular values of the jacobian that are < eps;
* if the number of near zero singular values is > jac.col()-jac.row(),
* then the jacobian pseudoinverse is singular
*/
unsigned int getNrZeroSigmas()const {return nrZeroSigmas;};
/**
* Request the minimum of the first six singular values
*/
double getSigmaMin()const {return sigmaMin;};
/**
* Request the six singular values of the Jacobian
*/
int getSigma(Eigen::VectorXd& Sout);
/**
* Request the value of eps
*/
double getEps()const {return eps;};
/**
* Request the value of lambda for the minimum
*/
double getLambda()const {return lambda;};
/**
* Request the scaled value of lambda for the minimum
* singular value 1-6
*/
double getLambdaScaled()const {return lambda_scaled;};
/**
* Retrieve the latest return code from the SVD algorithm
* @return 0 if CartToJnt() not yet called, otherwise latest SVD result code.
*/
int getSVDResult()const {return svdResult;};
/// @copydoc KDL::SolverI::strError()
virtual const char* strError(const int error) const;
/// @copydoc KDL::SolverI::updateInternalDataStructures()
virtual void updateInternalDataStructures();
private:
const Chain& chain;
ChainJntToJacSolver jnt2jac;
unsigned int nj;
Jacobian jac;
Eigen::MatrixXd U;
Eigen::VectorXd S;
Eigen::MatrixXd V;
double eps;
int maxiter;
Eigen::VectorXd tmp;
Eigen::MatrixXd tmp_jac;
Eigen::MatrixXd tmp_jac_weight1;
Eigen::MatrixXd tmp_jac_weight2;
Eigen::MatrixXd tmp_ts;
Eigen::MatrixXd tmp_js;
Eigen::MatrixXd weight_ts;
Eigen::MatrixXd weight_js;
double lambda;
double lambda_scaled;
unsigned int nrZeroSigmas ;
int svdResult;
double sigmaMin;
};
}
#endif

183
kdl_install/include/kdl/chainjnttojacdotsolver.hpp Normal file
View File

@@ -0,0 +1,183 @@
/*
Computes the Jacobian time derivative
Copyright (C) 2015 Antoine Hoarau <hoarau [at] isir.upmc.fr>
This library 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 2.1 of the License, or (at your option) any later version.
This library 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
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*/
#ifndef KDL_CHAINJNTTOJACDOTSOLVER_HPP
#define KDL_CHAINJNTTOJACDOTSOLVER_HPP
#include "solveri.hpp"
#include "frames.hpp"
#include "jntarrayvel.hpp"
#include "jacobian.hpp"
#include "chain.hpp"
#include "framevel.hpp"
#include "chainjnttojacsolver.hpp"
#include "chainfksolverpos_recursive.hpp"
namespace KDL
{
/**
* @brief Computes the Jacobian time derivative (Jdot) by calculating the
* partial derivatives regarding to a joint angle, in the Hybrid, Body-fixed
* or Inertial representation.
*
* This work is based on :
* Symbolic differentiation of the velocity mapping for a serial kinematic chain
* H. Bruyninckx, J. De Schutter
* doi:10.1016/0094-114X(95)00069-B
*
* url : http://www.sciencedirect.com/science/article/pii/0094114X9500069B
*/
class ChainJntToJacDotSolver : public SolverI
{
public:
static const int E_JAC_DOT_FAILED = -100;
static const int E_JACSOLVER_FAILED = -101;
static const int E_FKSOLVERPOS_FAILED = -102;
// Hybrid representation ref Frame: base, ref Point: end-effector
static const int HYBRID = 0;
// Body-fixed representation ref Frame: end-effector, ref Point: end-effector
static const int BODYFIXED = 1;
// Inertial representation ref Frame: base, ref Point: base
static const int INERTIAL = 2;
explicit ChainJntToJacDotSolver(const Chain& chain);
virtual ~ChainJntToJacDotSolver();
/**
* @brief Computes \f$ {}_{bs}\dot{J}^{ee}.\dot{q} \f$
*
* @param q_in Current joint positions and velocities
* @param jac_dot_q_dot The twist representing Jdot*qdot
* @param seg_nr The final segment to compute
* @return int 0 if no errors happened
*/
virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Twist& jac_dot_q_dot, int seg_nr = -1);
/**
* @brief Computes \f$ {}_{bs}\dot{J}^{ee} \f$
*
* @param q_in Current joint positions and velocities
* @param jdot The jacobian time derivative in the configured representation
* (HYBRID, BODYFIXED or INERTIAL)
* @param seg_nr The final segment to compute
* @return int 0 if no errors happened
*/
virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Jacobian& jdot, int seg_nr = -1);
int setLockedJoints(const std::vector<bool>& locked_joints);
/**
* @brief JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector)
*
*
* @return void
*/
void setHybridRepresentation(){setRepresentation(HYBRID);}
/**
* @brief JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector, ref Point: end-effector)
*
* @return void
*/
void setBodyFixedRepresentation(){setRepresentation(BODYFIXED);}
/**
* @brief JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base)
*
* @return void
*/
void setInertialRepresentation(){setRepresentation(INERTIAL);}
/**
* @brief Sets the internal variable for the representation (with a check on the value)
*
* @param representation The representation for Jdot : HYBRID,BODYFIXED or INERTIAL
* @return void
*/
void setRepresentation(const int& representation);
/// @copydoc KDL::SolverI::updateInternalDataStructures
virtual void updateInternalDataStructures();
/// @copydoc KDL::SolverI::strError()
virtual const char* strError(const int error) const;
protected:
/**
* @brief Computes \f$ \frac{\partial {}_{bs}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
*
* @param bs_J_ee The Jacobian expressed in the base frame with the end effector as the reference point (default in KDL Jacobian Solver)
* @param joint_idx The index of the current joint (j in the formula)
* @param column_idx The index of the current column (i in the formula)
* @return Twist The twist representing dJi/dqj .qdotj
*/
const Twist& getPartialDerivativeHybrid(const Jacobian& bs_J_ee,
const unsigned int& joint_idx,
const unsigned int& column_idx);
/**
* @brief Computes \f$ \frac{\partial {}_{ee}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
*
* @param bs_J_ee The Jacobian expressed in the end effector frame with the end effector as the reference point
* @param joint_idx The indice of the current joint (j in the formula)
* @param column_idx The indice of the current column (i in the formula)
* @return Twist The twist representing dJi/dqj .qdotj
*/
const Twist& getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee,
const unsigned int& joint_idx,
const unsigned int& column_idx);
/**
* @brief Computes \f$ \frac{\partial {}_{bs}J^{i,bs}}{\partial q^{j}}.\dot{q}^{j} \f$
*
* @param ee_J_ee The Jacobian expressed in the base frame with the base as the reference point
* @param joint_idx The indice of the current joint (j in the formula)
* @param column_idx The indice of the current column (i in the formula)
* @return Twist The twist representing dJi/dqj .qdotj
*/
const Twist& getPartialDerivativeInertial(const Jacobian& bs_J_bs,
const unsigned int& joint_idx,
const unsigned int& column_idx);
/**
* @brief Computes \f$ \frac{\partial J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
*
* @param bs_J_bs The Jacobian expressed in the base frame with the end effector as the reference point
* @param joint_idx The indice of the current joint (j in the formula)
* @param column_idx The indice of the current column (i in the formula)
* @param representation The representation (Hybrid,Body-fixed,Inertial) in which you want to get dJ/dqj .qdotj
* @return Twist The twist representing dJi/dqj .qdotj
*/
const Twist& getPartialDerivative(const Jacobian& J,
const unsigned int& joint_idx,
const unsigned int& column_idx,
const int& representation);
private:
const Chain& chain;
std::vector<bool> locked_joints_;
unsigned int nr_of_unlocked_joints_;
ChainJntToJacSolver jac_solver_;
Jacobian jac_;
Jacobian jac_dot_;
int representation_;
ChainFkSolverPos_recursive fk_solver_;
Frame F_bs_ee_;
Twist jac_dot_k_;
Twist jac_j_, jac_i_;
Twist t_djdq_;
};
}
#endif

74
kdl_install/include/kdl/chainjnttojacsolver.hpp Normal file
View File

@@ -0,0 +1,74 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CHAINJNTTOJACSOLVER_HPP
#define KDL_CHAINJNTTOJACSOLVER_HPP
#include "solveri.hpp"
#include "frames.hpp"
#include "jacobian.hpp"
#include "jntarray.hpp"
#include "chain.hpp"
namespace KDL
{
/**
* @brief Class to calculate the jacobian of a general
* KDL::Chain, it is used by other solvers.
*/
class ChainJntToJacSolver : public SolverI
{
public:
explicit ChainJntToJacSolver(const Chain& chain);
virtual ~ChainJntToJacSolver();
/**
* Calculate the jacobian expressed in the base frame of the
* chain, with reference point at the end effector of the
* *chain. The algorithm is similar to the one used in
* KDL::ChainFkSolverVel_recursive
*
* @param q_in input joint positions
* @param jac output jacobian
* @param seg_nr The final segment to compute
* @return success/error code
*/
virtual int JntToJac(const JntArray& q_in, Jacobian& jac, int seg_nr=-1);
/**
*
* @param locked_joints new values for locked joints
* @return success/error code
*/
int setLockedJoints(const std::vector<bool> locked_joints);
/// @copydoc KDL::SolverI::updateInternalDataStructures
virtual void updateInternalDataStructures();
private:
const Chain& chain;
Twist t_tmp;
Frame T_tmp;
std::vector<bool> locked_joints_;
};
}
#endif

37
kdl_install/include/kdl/config.h Normal file
View File

@@ -0,0 +1,37 @@
// Copyright (C) 2014 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Brian Jensen <Jensen dot J dot Brian at gmail dot com>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_CONFIG_H
#define KDL_CONFIG_H
#define KDL_VERSION_MAJOR 1
#define KDL_VERSION_MINOR 5
#define KDL_VERSION_PATCH 4
#define KDL_VERSION (KDL_VERSION_MAJOR << 16) | (KDL_VERSION_MINOR << 8) | KDL_VERSION_PATCH
#define KDL_VERSION_STRING "1.5.4"
//Set which version of the Tree Interface to use
#define HAVE_STL_CONTAINER_INCOMPLETE_TYPES
/* #undef KDL_USE_NEW_TREE_INTERFACE */
#endif //#define KDL_CONFIG_H

315
kdl_install/include/kdl/frameacc.hpp Normal file
View File

@@ -0,0 +1,315 @@
/*****************************************************************************
* \file
* This file contains the definition of classes for a
* Rall Algebra of (subset of) the classes defined in frames,
* i.e. classes that contain a set (value,derivative,2nd derivative)
* and define operations on that set
* this classes are useful for automatic differentiation ( <-> symbolic diff ,
* <-> numeric diff).
* Defines VectorAcc, RotationAcc, FrameAcc, doubleAcc.
* Look at the corresponding classes Vector Rotation Frame Twist and
* Wrench for the semantics of the methods.
*
* It also contains the 2nd derivative <-> RFrames.h
*
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: rrframes.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
* $Name: $
****************************************************************************/
#ifndef RRFRAMES_H
#define RRFRAMES_H
#include "utilities/rall2d.h"
#include "frames.hpp"
namespace KDL {
class TwistAcc;
typedef Rall2d<double,double,double> doubleAcc;
// Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4)
class FrameAcc;
class RotationAcc;
class VectorAcc;
IMETHOD bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon);
IMETHOD bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon);
IMETHOD bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon);
IMETHOD bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon);
IMETHOD bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon);
IMETHOD bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon);
IMETHOD bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon);
IMETHOD bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon);
IMETHOD bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon);
IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon);
IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon);
IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon);
class VectorAcc
{
public:
Vector p; //!< position vector
Vector v; //!< velocity vector
Vector dv; //!< acceleration vector
public:
VectorAcc():p(),v(),dv() {}
explicit VectorAcc(const Vector& _p):p(_p),v(Vector::Zero()),dv(Vector::Zero()) {}
VectorAcc(const Vector& _p,const Vector& _v):p(_p),v(_v),dv(Vector::Zero()) {}
VectorAcc(const Vector& _p,const Vector& _v,const Vector& _dv):
p(_p),v(_v),dv(_dv) {}
IMETHOD VectorAcc& operator = (const VectorAcc& arg);
IMETHOD VectorAcc& operator = (const Vector& arg);
IMETHOD VectorAcc& operator += (const VectorAcc& arg);
IMETHOD VectorAcc& operator -= (const VectorAcc& arg);
IMETHOD static VectorAcc Zero();
IMETHOD void ReverseSign();
IMETHOD doubleAcc Norm(double eps=epsilon);
IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2);
IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2);
IMETHOD friend VectorAcc operator + (const Vector& r1,const VectorAcc& r2);
IMETHOD friend VectorAcc operator - (const Vector& r1,const VectorAcc& r2);
IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const Vector& r2);
IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const Vector& r2);
IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2);
IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const Vector& r2);
IMETHOD friend VectorAcc operator * (const Vector& r1,const VectorAcc& r2);
IMETHOD friend VectorAcc operator * (const VectorAcc& r1,double r2);
IMETHOD friend VectorAcc operator * (double r1,const VectorAcc& r2);
IMETHOD friend VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2);
IMETHOD friend VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1);
IMETHOD friend VectorAcc operator*(const Rotation& R,const VectorAcc& x);
IMETHOD friend VectorAcc operator / (const VectorAcc& r1,double r2);
IMETHOD friend VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1);
IMETHOD friend bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps);
IMETHOD friend bool Equal(const Vector& r1,const VectorAcc& r2,double eps);
IMETHOD friend bool Equal(const VectorAcc& r1,const Vector& r2,double eps);
IMETHOD friend VectorAcc operator - (const VectorAcc& r);
IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs);
IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const Vector& rhs);
IMETHOD friend doubleAcc dot(const Vector& lhs,const VectorAcc& rhs);
};
class RotationAcc
{
public:
Rotation R; //!< rotation matrix
Vector w; //!< angular velocity vector
Vector dw; //!< angular acceration vector
public:
RotationAcc():R(),w() {}
explicit RotationAcc(const Rotation& _R):R(_R),w(Vector::Zero()){}
RotationAcc(const Rotation& _R,const Vector& _w,const Vector& _dw):
R(_R),w(_w),dw(_dw) {}
IMETHOD RotationAcc& operator = (const RotationAcc& arg);
IMETHOD RotationAcc& operator = (const Rotation& arg);
IMETHOD static RotationAcc Identity();
IMETHOD RotationAcc Inverse() const;
IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
IMETHOD VectorAcc Inverse(const Vector& arg) const;
IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
IMETHOD VectorAcc operator*(const Vector& arg) const;
// Rotations
// The SetRot.. functions set the value of *this to the appropriate rotation matrix.
// The Rot... static functions give the value of the appropriate rotation matrix back.
// The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
// IMETHOD void DoRotX(const doubleAcc& angle);
// IMETHOD void DoRotY(const doubleAcc& angle);
// IMETHOD void DoRotZ(const doubleAcc& angle);
// IMETHOD static RRotation RotX(const doubleAcc& angle);
// IMETHOD static RRotation RotY(const doubleAcc& angle);
// IMETHOD static RRotation RotZ(const doubleAcc& angle);
// IMETHOD void SetRot(const Vector& rotaxis,const doubleAcc& angle);
// Along an arbitrary axes. The norm of rotvec is neglected.
// IMETHOD static RotationAcc Rot(const Vector& rotvec,const doubleAcc& angle);
// rotvec has arbitrary norm
// rotation around a constant vector !
// IMETHOD static RotationAcc Rot2(const Vector& rotvec,const doubleAcc& angle);
// rotvec is normalized.
// rotation around a constant vector !
IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2);
IMETHOD friend RotationAcc operator* (const Rotation& r1,const RotationAcc& r2);
IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const Rotation& r2);
IMETHOD friend bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps);
IMETHOD friend bool Equal(const Rotation& r1,const RotationAcc& r2,double eps);
IMETHOD friend bool Equal(const RotationAcc& r1,const Rotation& r2,double eps);
IMETHOD TwistAcc Inverse(const TwistAcc& arg) const;
IMETHOD TwistAcc Inverse(const Twist& arg) const;
IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
IMETHOD TwistAcc operator * (const Twist& arg) const;
};
class FrameAcc
{
public:
RotationAcc M; //!< Rotation,angular velocity, and angular acceleration of frame.
VectorAcc p; //!< Translation, velocity and acceleration of origin.
public:
FrameAcc(){}
explicit FrameAcc(const Frame& _T):M(_T.M),p(_T.p) {}
FrameAcc(const Frame& _T,const Twist& _t,const Twist& _dt):
M(_T.M,_t.rot,_dt.rot),p(_T.p,_t.vel,_dt.vel) {}
FrameAcc(const RotationAcc& _M,const VectorAcc& _p):M(_M),p(_p) {}
IMETHOD FrameAcc& operator = (const FrameAcc& arg);
IMETHOD FrameAcc& operator = (const Frame& arg);
IMETHOD static FrameAcc Identity();
IMETHOD FrameAcc Inverse() const;
IMETHOD VectorAcc Inverse(const VectorAcc& arg) const;
IMETHOD VectorAcc operator*(const VectorAcc& arg) const;
IMETHOD VectorAcc operator*(const Vector& arg) const;
IMETHOD VectorAcc Inverse(const Vector& arg) const;
IMETHOD Frame GetFrame() const;
IMETHOD Twist GetTwist() const;
IMETHOD Twist GetAccTwist() const;
IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const FrameAcc& f2);
IMETHOD friend FrameAcc operator * (const Frame& f1,const FrameAcc& f2);
IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const Frame& f2);
IMETHOD friend bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps);
IMETHOD friend bool Equal(const Frame& r1,const FrameAcc& r2,double eps);
IMETHOD friend bool Equal(const FrameAcc& r1,const Frame& r2,double eps);
IMETHOD TwistAcc Inverse(const TwistAcc& arg) const;
IMETHOD TwistAcc Inverse(const Twist& arg) const;
IMETHOD TwistAcc operator * (const TwistAcc& arg) const;
IMETHOD TwistAcc operator * (const Twist& arg) const;
};
//very similar to Wrench class.
class TwistAcc
{
public:
VectorAcc vel; //!< translational velocity and its 1st and 2nd derivative
VectorAcc rot; //!< rotational velocity and its 1st and 2nd derivative
public:
TwistAcc():vel(),rot() {};
TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {};
IMETHOD TwistAcc& operator-=(const TwistAcc& arg);
IMETHOD TwistAcc& operator+=(const TwistAcc& arg);
IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,double rhs);
IMETHOD friend TwistAcc operator*(double lhs,const TwistAcc& rhs);
IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,double rhs);
IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs);
IMETHOD friend TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs);
IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs);
IMETHOD friend TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs);
IMETHOD friend TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs);
IMETHOD friend TwistAcc operator-(const TwistAcc& arg);
IMETHOD friend void SetToZero(TwistAcc& v);
static IMETHOD TwistAcc Zero();
IMETHOD void ReverseSign();
IMETHOD TwistAcc RefPoint(const VectorAcc& v_base_AB);
// Changes the reference point of the RTwist.
// The RVector v_base_AB is expressed in the same base as the RTwist
// The RVector v_base_AB is a RVector from the old point to
// the new point.
// Complexity : 6M+6A
IMETHOD friend bool Equal(const TwistAcc& a,const TwistAcc& b,double eps);
IMETHOD friend bool Equal(const Twist& a,const TwistAcc& b,double eps);
IMETHOD friend bool Equal(const TwistAcc& a,const Twist& b,double eps);
IMETHOD Twist GetTwist() const;
IMETHOD Twist GetTwistDot() const;
friend class RotationAcc;
friend class FrameAcc;
};
#ifdef KDL_INLINE
#include "frameacc.inl"
#endif
} // namespace KDL
template<> struct std::hash<KDL::doubleAcc>
{
std::size_t operator()(KDL::doubleAcc const& da) const noexcept
{
size_t seed = 0;
KDL::hash_combine(seed, da.t);
KDL::hash_combine(seed, da.d);
KDL::hash_combine(seed, da.dd);
return seed;
}
};
template<> struct std::hash<KDL::VectorAcc>
{
std::size_t operator()(KDL::VectorAcc const& va) const noexcept
{
size_t seed = 0;
KDL::hash_combine(seed, va.p);
KDL::hash_combine(seed, va.v);
KDL::hash_combine(seed, va.dv);
return seed;
}
};
template<> struct std::hash<KDL::RotationAcc>
{
std::size_t operator()(KDL::RotationAcc const& ra) const noexcept
{
size_t seed = 0;
KDL::hash_combine(seed, ra.R);
KDL::hash_combine(seed, ra.w);
KDL::hash_combine(seed, ra.dw);
return seed;
}
};
template<> struct std::hash<KDL::FrameAcc>
{
std::size_t operator()(KDL::FrameAcc const& fa) const noexcept
{
size_t seed = 0;
KDL::hash_combine(seed, fa.M);
KDL::hash_combine(seed, fa.p);
return seed;
}
};
template<> struct std::hash<KDL::TwistAcc>
{
std::size_t operator()(KDL::TwistAcc const& ta) const noexcept
{
size_t seed = 0;
KDL::hash_combine(seed, ta.vel);
KDL::hash_combine(seed, ta.rot);
return seed;
}
};
#endif

598
kdl_install/include/kdl/frameacc.inl Normal file
View File

@@ -0,0 +1,598 @@
/*****************************************************************************
* \file
* provides inline functions of rrframes.h
*
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: rrframes.inl,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
* $Name: $
****************************************************************************/
/////////////////// VectorAcc /////////////////////////////////////
VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2) {
return VectorAcc(r1.p+r2.p,r1.v+r2.v,r1.dv+r2.dv);
}
VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2) {
return VectorAcc(r1.p-r2.p, r1.v-r2.v, r1.dv-r2.dv);
}
VectorAcc operator + (const Vector& r1,const VectorAcc& r2) {
return VectorAcc(r1+r2.p,r2.v,r2.dv);
}
VectorAcc operator - (const Vector& r1,const VectorAcc& r2) {
return VectorAcc(r1-r2.p, -r2.v, -r2.dv);
}
VectorAcc operator + (const VectorAcc& r1,const Vector& r2) {
return VectorAcc(r1.p+r2,r1.v,r1.dv);
}
VectorAcc operator - (const VectorAcc& r1,const Vector& r2) {
return VectorAcc(r1.p-r2, r1.v, r1.dv);
}
// unary -
VectorAcc operator - (const VectorAcc& r) {
return VectorAcc(-r.p,-r.v,-r.dv);
}
// cross prod.
VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2) {
return VectorAcc(r1.p*r2.p,
r1.p*r2.v+r1.v*r2.p,
r1.dv*r2.p+2*r1.v*r2.v+r1.p*r2.dv
);
}
VectorAcc operator * (const VectorAcc& r1,const Vector& r2) {
return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 );
}
VectorAcc operator * (const Vector& r1,const VectorAcc& r2) {
return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv );
}
// scalar mult.
VectorAcc operator * (double r1,const VectorAcc& r2) {
return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv );
}
VectorAcc operator * (const VectorAcc& r1,double r2) {
return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 );
}
VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2) {
return VectorAcc(r1.t*r2.p,
r1.t*r2.v + r1.d*r2.p,
r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p
);
}
VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1) {
return VectorAcc(r1.t*r2.p,
r1.t*r2.v + r1.d*r2.p,
r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p
);
}
VectorAcc& VectorAcc::operator = (const VectorAcc& arg) {
p=arg.p;
v=arg.v;
dv=arg.dv;
return *this;
}
VectorAcc& VectorAcc::operator = (const Vector& arg) {
p=arg;
v=Vector::Zero();
dv=Vector::Zero();
return *this;
}
VectorAcc& VectorAcc::operator += (const VectorAcc& arg) {
p+=arg.p;
v+=arg.v;
dv+= arg.dv;
return *this;
}
VectorAcc& VectorAcc::operator -= (const VectorAcc& arg) {
p-=arg.p;
v-=arg.v;
dv-=arg.dv;
return *this;
}
VectorAcc VectorAcc::Zero() {
return VectorAcc(Vector::Zero(),Vector::Zero(),Vector::Zero());
}
void VectorAcc::ReverseSign() {
p.ReverseSign();
v.ReverseSign();
dv.ReverseSign();
}
doubleAcc VectorAcc::Norm(double eps) {
doubleAcc res;
res.t = p.Norm(eps);
res.d = dot(p,v)/res.t;
res.dd = (dot(p,dv)+dot(v,v)-res.d*res.d)/res.t;
return res;
}
doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs) {
return doubleAcc( dot(lhs.p,rhs.p),
dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p),
dot(lhs.p,rhs.dv)+2*dot(lhs.v,rhs.v)+dot(lhs.dv,rhs.p)
);
}
doubleAcc dot(const VectorAcc& lhs,const Vector& rhs) {
return doubleAcc( dot(lhs.p,rhs),
dot(lhs.v,rhs),
dot(lhs.dv,rhs)
);
}
doubleAcc dot(const Vector& lhs,const VectorAcc& rhs) {
return doubleAcc( dot(lhs,rhs.p),
dot(lhs,rhs.v),
dot(lhs,rhs.dv)
);
}
bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps) {
return (Equal(r1.p,r2.p,eps)
&& Equal(r1.v,r2.v,eps)
&& Equal(r1.dv,r2.dv,eps)
);
}
bool Equal(const Vector& r1,const VectorAcc& r2,double eps) {
return (Equal(r1,r2.p,eps)
&& Equal(Vector::Zero(),r2.v,eps)
&& Equal(Vector::Zero(),r2.dv,eps)
);
}
bool Equal(const VectorAcc& r1,const Vector& r2,double eps) {
return (Equal(r1.p,r2,eps)
&& Equal(r1.v,Vector::Zero(),eps)
&& Equal(r1.dv,Vector::Zero(),eps)
);
}
VectorAcc operator / (const VectorAcc& r1,double r2) {
return r1*(1.0/r2);
}
VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1) {
return r2*(1.0/r1);
}
/////////////////// RotationAcc /////////////////////////////////////
RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2) {
return RotationAcc( r1.R * r2.R,
r1.w + r1.R*r2.w,
r1.dw + r1.w*(r1.R*r2.w) + r1.R*r2.dw
);
}
RotationAcc operator* (const Rotation& r1,const RotationAcc& r2) {
return RotationAcc( r1*r2.R, r1*r2.w, r1*r2.dw);
}
RotationAcc operator* (const RotationAcc& r1,const Rotation& r2) {
return RotationAcc( r1.R*r2, r1.w, r1.dw );
}
RotationAcc& RotationAcc::operator = (const RotationAcc& arg) {
R=arg.R;
w=arg.w;
dw=arg.dw;
return *this;
}
RotationAcc& RotationAcc::operator = (const Rotation& arg) {
R = arg;
w = Vector::Zero();
dw = Vector::Zero();
return *this;
}
RotationAcc RotationAcc::Identity() {
return RotationAcc(Rotation::Identity(),Vector::Zero(),Vector::Zero());
}
RotationAcc RotationAcc::Inverse() const {
return RotationAcc(R.Inverse(),-R.Inverse(w),-R.Inverse(dw));
}
VectorAcc RotationAcc::Inverse(const VectorAcc& arg) const {
VectorAcc tmp;
tmp.p = R.Inverse(arg.p);
tmp.v = R.Inverse(arg.v - w * arg.p);
tmp.dv = R.Inverse(arg.dv - dw*arg.p - w*(arg.v+R*tmp.v));
return tmp;
}
VectorAcc RotationAcc::Inverse(const Vector& arg) const {
VectorAcc tmp;
tmp.p = R.Inverse(arg);
tmp.v = R.Inverse(-w*arg);
tmp.dv = R.Inverse(-dw*arg - w*(R*tmp.v));
return tmp;
}
VectorAcc RotationAcc::operator*(const VectorAcc& arg) const {
VectorAcc tmp;
tmp.p = R*arg.p;
tmp.dv = R*arg.v;
tmp.v = w*tmp.p + tmp.dv;
tmp.dv = dw*tmp.p + w*(tmp.v + tmp.dv) + R*arg.dv;
return tmp;
}
VectorAcc operator*(const Rotation& R,const VectorAcc& x) {
return VectorAcc(R*x.p,R*x.v,R*x.dv);
}
VectorAcc RotationAcc::operator*(const Vector& arg) const {
VectorAcc tmp;
tmp.p = R*arg;
tmp.v = w*tmp.p;
tmp.dv = dw*tmp.p + w*tmp.v;
return tmp;
}
/*
// = Rotations
// The Rot... static functions give the value of the appropriate rotation matrix back.
// The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
void RRotation::DoRotX(const RDouble& angle) {
w+=R*Vector(angle.grad,0,0);
R.DoRotX(angle.t);
}
RotationAcc RotationAcc::RotX(const doubleAcc& angle) {
return RotationAcc(Rotation::RotX(angle.t),
Vector(angle.d,0,0),
Vector(angle.dd,0,0)
);
}
void RRotation::DoRotY(const RDouble& angle) {
w+=R*Vector(0,angle.grad,0);
R.DoRotY(angle.t);
}
RotationAcc RotationAcc::RotY(const doubleAcc& angle) {
return RotationAcc(
Rotation::RotX(angle.t),
Vector(0,angle.d,0),
Vector(0,angle.dd,0)
);
}
void RRotation::DoRotZ(const RDouble& angle) {
w+=R*Vector(0,0,angle.grad);
R.DoRotZ(angle.t);
}
RotationAcc RotationAcc::RotZ(const doubleAcc& angle) {
return RotationAcc(
Rotation::RotZ(angle.t),
Vector(0,0,angle.d),
Vector(0,0,angle.dd)
);
}
RRotation RRotation::Rot(const Vector& rotvec,const RDouble& angle)
// rotvec has arbitrary norm
// rotation around a constant vector !
{
Vector v = rotvec.Normalize();
return RRotation(Rotation::Rot2(v,angle.t),v*angle.grad);
}
RRotation RRotation::Rot2(const Vector& rotvec,const RDouble& angle)
// rotvec is normalized.
{
return RRotation(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad);
}
*/
bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps) {
return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps) && Equal(r1.dw,r2.dw,eps) );
}
bool Equal(const Rotation& r1,const RotationAcc& r2,double eps) {
return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps) &&
Equal(Vector::Zero(),r2.dw,eps) );
}
bool Equal(const RotationAcc& r1,const Rotation& r2,double eps) {
return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps) &&
Equal(r1.dw,Vector::Zero(),eps) );
}
// Methods and operators related to FrameAcc
// They all delegate most of the work to RotationAcc and VectorAcc
FrameAcc& FrameAcc::operator = (const FrameAcc& arg) {
M=arg.M;
p=arg.p;
return *this;
}
FrameAcc FrameAcc::Identity() {
return FrameAcc(RotationAcc::Identity(),VectorAcc::Zero());
}
FrameAcc operator *(const FrameAcc& lhs,const FrameAcc& rhs)
{
return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
}
FrameAcc operator *(const FrameAcc& lhs,const Frame& rhs)
{
return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
}
FrameAcc operator *(const Frame& lhs,const FrameAcc& rhs)
{
return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
}
VectorAcc FrameAcc::operator *(const VectorAcc & arg) const
{
return M*arg+p;
}
VectorAcc FrameAcc::operator *(const Vector & arg) const
{
return M*arg+p;
}
VectorAcc FrameAcc::Inverse(const VectorAcc& arg) const
{
return M.Inverse(arg-p);
}
VectorAcc FrameAcc::Inverse(const Vector& arg) const
{
return M.Inverse(arg-p);
}
FrameAcc FrameAcc::Inverse() const
{
return FrameAcc(M.Inverse(),-M.Inverse(p));
}
FrameAcc& FrameAcc::operator =(const Frame & arg)
{
M = arg.M;
p = arg.p;
return *this;
}
bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps) {
return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
}
bool Equal(const Frame& r1,const FrameAcc& r2,double eps) {
return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
}
bool Equal(const FrameAcc& r1,const Frame& r2,double eps) {
return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
}
Frame FrameAcc::GetFrame() const {
return Frame(M.R,p.p);
}
Twist FrameAcc::GetTwist() const {
return Twist(p.v,M.w);
}
Twist FrameAcc::GetAccTwist() const {
return Twist(p.dv,M.dw);
}
TwistAcc TwistAcc::Zero()
{
return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero());
}
void TwistAcc::ReverseSign()
{
vel.ReverseSign();
rot.ReverseSign();
}
TwistAcc TwistAcc::RefPoint(const VectorAcc& v_base_AB)
// Changes the reference point of the TwistAcc.
// The RVector v_base_AB is expressed in the same base as the TwistAcc
// The RVector v_base_AB is a RVector from the old point to
// the new point.
// Complexity : 6M+6A
{
return TwistAcc(this->vel+this->rot*v_base_AB,this->rot);
}
TwistAcc& TwistAcc::operator-=(const TwistAcc& arg)
{
vel-=arg.vel;
rot -=arg.rot;
return *this;
}
TwistAcc& TwistAcc::operator+=(const TwistAcc& arg)
{
vel+=arg.vel;
rot +=arg.rot;
return *this;
}
TwistAcc operator*(const TwistAcc& lhs,double rhs)
{
return TwistAcc(lhs.vel*rhs,lhs.rot*rhs);
}
TwistAcc operator*(double lhs,const TwistAcc& rhs)
{
return TwistAcc(lhs*rhs.vel,lhs*rhs.rot);
}
TwistAcc operator/(const TwistAcc& lhs,double rhs)
{
return TwistAcc(lhs.vel/rhs,lhs.rot/rhs);
}
TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs)
{
return TwistAcc(lhs.vel*rhs,lhs.rot*rhs);
}
TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs)
{
return TwistAcc(lhs*rhs.vel,lhs*rhs.rot);
}
TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs)
{
return TwistAcc(lhs.vel/rhs,lhs.rot/rhs);
}
// addition of TwistAcc's
TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs)
{
return TwistAcc(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
}
TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs)
{
return TwistAcc(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
}
// unary -
TwistAcc operator-(const TwistAcc& arg)
{
return TwistAcc(-arg.vel,-arg.rot);
}
TwistAcc RotationAcc::Inverse(const TwistAcc& arg) const
{
return TwistAcc(Inverse(arg.vel),Inverse(arg.rot));
}
TwistAcc RotationAcc::operator * (const TwistAcc& arg) const
{
return TwistAcc((*this)*arg.vel,(*this)*arg.rot);
}
TwistAcc RotationAcc::Inverse(const Twist& arg) const
{
return TwistAcc(Inverse(arg.vel),Inverse(arg.rot));
}
TwistAcc RotationAcc::operator * (const Twist& arg) const
{
return TwistAcc((*this)*arg.vel,(*this)*arg.rot);
}
TwistAcc FrameAcc::operator * (const TwistAcc& arg) const
{
TwistAcc tmp;
tmp.rot = M*arg.rot;
tmp.vel = M*arg.vel+p*tmp.rot;
return tmp;
}
TwistAcc FrameAcc::operator * (const Twist& arg) const
{
TwistAcc tmp;
tmp.rot = M*arg.rot;
tmp.vel = M*arg.vel+p*tmp.rot;
return tmp;
}
TwistAcc FrameAcc::Inverse(const TwistAcc& arg) const
{
TwistAcc tmp;
tmp.rot = M.Inverse(arg.rot);
tmp.vel = M.Inverse(arg.vel-p*arg.rot);
return tmp;
}
TwistAcc FrameAcc::Inverse(const Twist& arg) const
{
TwistAcc tmp;
tmp.rot = M.Inverse(arg.rot);
tmp.vel = M.Inverse(arg.vel-p*arg.rot);
return tmp;
}
Twist TwistAcc::GetTwist() const {
return Twist(vel.p,rot.p);
}
Twist TwistAcc::GetTwistDot() const {
return Twist(vel.v,rot.v);
}
bool Equal(const TwistAcc& a,const TwistAcc& b,double eps) {
return (Equal(a.rot,b.rot,eps)&&
Equal(a.vel,b.vel,eps) );
}
bool Equal(const Twist& a,const TwistAcc& b,double eps) {
return (Equal(a.rot,b.rot,eps)&&
Equal(a.vel,b.vel,eps) );
}
bool Equal(const TwistAcc& a,const Twist& b,double eps) {
return (Equal(a.rot,b.rot,eps)&&
Equal(a.vel,b.vel,eps) );
}

58
kdl_install/include/kdl/frameacc_io.hpp Normal file
View File

@@ -0,0 +1,58 @@
/*****************************************************************************
* \file
* Defines I/O related routines to the FrameAccs classes defined in
* FrameAccs.h
*
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: rrframes_io.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
* $Name: $
****************************************************************************/
#ifndef RRFRAMES_IO
#define RRFRAMES_IO
#include "utilities/utility_io.h"
#include "utilities/rall2d_io.h"
#include "frames_io.hpp"
#include "frameacc.hpp"
namespace KDL {
// Output...
inline std::ostream& operator << (std::ostream& os,const VectorAcc& r) {
os << "{" << r.p << "," << r.v << "," << r.dv << "}" << std::endl;
return os;
}
inline std::ostream& operator << (std::ostream& os,const RotationAcc& r) {
os << "{" << std::endl << r.R << "," << std::endl << r.w <<
"," << std::endl << r.dw << std::endl << "}" << std::endl;
return os;
}
inline std::ostream& operator << (std::ostream& os,const FrameAcc& r) {
os << "{" << std::endl << r.M << "," << std::endl << r.p << "}" << std::endl;
return os;
}
inline std::ostream& operator << (std::ostream& os,const TwistAcc& r) {
os << "{" << std::endl << r.vel << "," << std::endl << r.rot << std::endl << "}" << std::endl;
return os;
}
} // namespace Frame
#endif

1354
kdl_install/include/kdl/frames.hpp Normal file
View File

File diff suppressed because it is too large Load Diff

1343
kdl_install/include/kdl/frames.inl Normal file
View File

File diff suppressed because it is too large Load Diff

114
kdl_install/include/kdl/frames_io.hpp Normal file
View File

@@ -0,0 +1,114 @@
/***************************************************************************
frames_io.h - description
-------------------------
begin : June 2006
copyright : (C) 2006 Erwin Aertbelien
email : firstname.lastname@mech.kuleuven.ac.be
History (only major changes)( AUTHOR-Description ) :
Ruben Smits - Added output for jacobian and jntarray 06/2007
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/**
//
// \file
// Defines routines for I/O of Frame and related objects.
// \verbatim
// Spaces, tabs and newlines do not have any importance.
// Comments are allowed C-style,C++-style, make/perl/csh -style
// Description of the I/O :
// Vector : OUTPUT : e.g. [10,20,30]
// INPUT :
// 1) [10,20,30]
// 2) Zero
// Twist : e.g. [1,2,3,4,5,6]
// where [1,2,3] is velocity vector
// where [4,5,6] is rotational velocity vector
// Wrench : e.g. [1,2,3,4,5,6]
// where [1,2,3] represents a force vector
// where [4,5,6] represents a torque vector
// Rotation : output :
// [1,2,3;
// 4,5,6;
// 7,8,9] cfr definition of Rotation object.
// input :
// 1) like the output
// 2) EulerZYX,EulerZYZ,RPY word followed by a vector, e.g. :
// Eulerzyx[10,20,30]
// (ANGLES are always expressed in DEGREES for I/O)
// (ANGELS are always expressed in RADIANS for internal representation)
// 3) Rot [1,2,3] [20] Rotates around axis [1,2,3] with an angle
// of 20 degrees.
// 4) Identity returns identity rotation matrix.
// Frames : output : [ Rotationmatrix positionvector ]
// e.g. [ [1,0,0;0,1,0;0,0,1] [1,2,3] ]
// Input :
// 1) [ Rotationmatrix positionvector ]
// 2) DH [ 10,10,50,30] Denavit-Hartenberg representation
// ( is in fact not the representation of a Frame, but more
// limited, cfr. documentation of Frame object.)
// \endverbatim
//
// \warning
// You can use iostream.h or iostream header files for file I/O,
// if one declares the define WANT_STD_IOSTREAM then the standard C++
// iostreams headers are included instead of the compiler-dependent version
//
*
****************************************************************************/
#ifndef FRAMES_IO_H
#define FRAMES_IO_H
#include "utilities/utility_io.h"
#include "frames.hpp"
#include "jntarray.hpp"
#include "jacobian.hpp"
namespace KDL {
//! width to be used when printing variables out with frames_io.h
//! global variable, can be changed.
// I/O to C++ stream.
std::ostream& operator << (std::ostream& os,const Vector& v);
std::ostream& operator << (std::ostream& os,const Rotation& R);
std::ostream& operator << (std::ostream& os,const Frame& T);
std::ostream& operator << (std::ostream& os,const Twist& T);
std::ostream& operator << (std::ostream& os,const Wrench& T);
std::ostream& operator << (std::ostream& os,const Vector2& v);
std::ostream& operator << (std::ostream& os,const Rotation2& R);
std::ostream& operator << (std::ostream& os,const Frame2& T);
std::istream& operator >> (std::istream& is,Vector& v);
std::istream& operator >> (std::istream& is,Rotation& R);
std::istream& operator >> (std::istream& is,Frame& T);
std::istream& operator >> (std::istream& os,Twist& T);
std::istream& operator >> (std::istream& os,Wrench& T);
std::istream& operator >> (std::istream& is,Vector2& v);
std::istream& operator >> (std::istream& is,Rotation2& R);
std::istream& operator >> (std::istream& is,Frame2& T);
} // namespace Frame
#endif

470
kdl_install/include/kdl/framevel.hpp Normal file
View File

@@ -0,0 +1,470 @@
/*****************************************************************************
* \file
* This file contains the definition of classes for a
* Rall Algebra of (subset of) the classes defined in frames,
* i.e. classes that contain a pair (value,derivative) and define operations on that pair
* this classes are useful for automatic differentiation ( <-> symbolic diff , <-> numeric diff)
* Defines VectorVel, RotationVel, FrameVel. Look at Frames.h for details on how to work
* with Frame objects.
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: rframes.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_FRAMEVEL_H
#define KDL_FRAMEVEL_H
#include "utilities/utility.h"
#include "utilities/rall1d.h"
#include "utilities/traits.h"
#include "frames.hpp"
namespace KDL {
typedef Rall1d<double> doubleVel;
IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) {
return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt);
}
IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) {
return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt);
}
IMETHOD void random(doubleVel& F) {
random(F.t);
random(F.grad);
}
IMETHOD void posrandom(doubleVel& F) {
posrandom(F.t);
posrandom(F.grad);
}
}
template <>
struct Traits<KDL::doubleVel> {
typedef double valueType;
typedef KDL::doubleVel derivType;
};
namespace KDL {
class TwistVel;
class VectorVel;
class FrameVel;
class RotationVel;
// Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4)
IMETHOD bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon);
IMETHOD bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon);
IMETHOD bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon);
IMETHOD bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon);
IMETHOD bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon);
IMETHOD bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon);
IMETHOD bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon);
IMETHOD bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon);
IMETHOD bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon);
IMETHOD bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon);
IMETHOD bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon);
IMETHOD bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon);
class VectorVel
// = TITLE
// An VectorVel is a Vector and its first derivative
// = CLASS TYPE
// Concrete
{
public:
Vector p; // position vector
Vector v; // velocity vector
public:
VectorVel():p(),v(){}
VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {}
explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {}
Vector value() const { return p;}
Vector deriv() const { return v;}
IMETHOD VectorVel& operator = (const VectorVel& arg);
IMETHOD VectorVel& operator = (const Vector& arg);
IMETHOD VectorVel& operator += (const VectorVel& arg);
IMETHOD VectorVel& operator -= (const VectorVel& arg);
IMETHOD static VectorVel Zero();
IMETHOD void ReverseSign();
IMETHOD doubleVel Norm(double eps=epsilon) const;
IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2);
IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2);
IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2);
IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2);
IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2);
IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2);
IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2);
IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2);
IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2);
IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2);
IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2);
IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2);
IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1);
IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x);
IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2);
IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1);
IMETHOD friend void SetToZero(VectorVel& v);
IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps);
IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps);
IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps);
IMETHOD friend bool operator==(const VectorVel& r1,const VectorVel& r2);
IMETHOD friend bool operator!=(const VectorVel& r1,const VectorVel& r2);
IMETHOD friend bool operator==(const Vector& r1,const VectorVel& r2);
IMETHOD friend bool operator!=(const Vector& r1,const VectorVel& r2);
IMETHOD friend bool operator==(const VectorVel& r1,const Vector& r2);
IMETHOD friend bool operator!=(const VectorVel& r1,const Vector& r2);
IMETHOD friend VectorVel operator - (const VectorVel& r);
IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs);
IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs);
IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs);
};
class RotationVel
// = TITLE
// An RotationVel is a Rotation and its first derivative, a rotation vector
// = CLASS TYPE
// Concrete
{
public:
Rotation R; // Rotation matrix
Vector w; // rotation vector
public:
RotationVel():R(),w() {}
explicit RotationVel(const Rotation& _R):R(_R),w(Vector::Zero()){}
RotationVel(const Rotation& _R,const Vector& _w):R(_R),w(_w){}
Rotation value() const { return R;}
Vector deriv() const { return w;}
IMETHOD RotationVel& operator = (const RotationVel& arg);
IMETHOD RotationVel& operator = (const Rotation& arg);
IMETHOD VectorVel UnitX() const;
IMETHOD VectorVel UnitY() const;
IMETHOD VectorVel UnitZ() const;
IMETHOD static RotationVel Identity();
IMETHOD RotationVel Inverse() const;
IMETHOD VectorVel Inverse(const VectorVel& arg) const;
IMETHOD VectorVel Inverse(const Vector& arg) const;
IMETHOD VectorVel operator*(const VectorVel& arg) const;
IMETHOD VectorVel operator*(const Vector& arg) const;
IMETHOD void DoRotX(const doubleVel& angle);
IMETHOD void DoRotY(const doubleVel& angle);
IMETHOD void DoRotZ(const doubleVel& angle);
IMETHOD static RotationVel RotX(const doubleVel& angle);
IMETHOD static RotationVel RotY(const doubleVel& angle);
IMETHOD static RotationVel RotZ(const doubleVel& angle);
IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle);
// rotvec has arbitrary norm
// rotation around a constant vector !
IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
// rotvec is normalized.
// rotation around a constant vector !
IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2);
IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2);
IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2);
IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps);
IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps);
IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps);
IMETHOD friend bool operator==(const RotationVel& r1,const RotationVel& r2);
IMETHOD friend bool operator!=(const RotationVel& r1,const RotationVel& r2);
IMETHOD friend bool operator==(const Rotation& r1,const RotationVel& r2);
IMETHOD friend bool operator!=(const Rotation& r1,const RotationVel& r2);
IMETHOD friend bool operator==(const RotationVel& r1,const Rotation& r2);
IMETHOD friend bool operator!=(const RotationVel& r1,const Rotation& r2);
IMETHOD TwistVel Inverse(const TwistVel& arg) const;
IMETHOD TwistVel Inverse(const Twist& arg) const;
IMETHOD TwistVel operator * (const TwistVel& arg) const;
IMETHOD TwistVel operator * (const Twist& arg) const;
};
class FrameVel
// = TITLE
// An FrameVel is a Frame and its first derivative, a Twist vector
// = CLASS TYPE
// Concrete
// = CAVEATS
//
{
public:
RotationVel M;
VectorVel p;
public:
FrameVel(){}
explicit FrameVel(const Frame& _T):
M(_T.M),p(_T.p) {}
FrameVel(const Frame& _T,const Twist& _t):
M(_T.M,_t.rot),p(_T.p,_t.vel) {}
FrameVel(const RotationVel& _M,const VectorVel& _p):
M(_M),p(_p) {}
Frame value() const { return Frame(M.value(),p.value());}
Twist deriv() const { return Twist(p.deriv(),M.deriv());}
IMETHOD FrameVel& operator = (const Frame& arg);
IMETHOD FrameVel& operator = (const FrameVel& arg);
IMETHOD static FrameVel Identity();
IMETHOD FrameVel Inverse() const;
IMETHOD VectorVel Inverse(const VectorVel& arg) const;
IMETHOD VectorVel operator*(const VectorVel& arg) const;
IMETHOD VectorVel operator*(const Vector& arg) const;
IMETHOD VectorVel Inverse(const Vector& arg) const;
IMETHOD Frame GetFrame() const;
IMETHOD Twist GetTwist() const;
IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2);
IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2);
IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2);
IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps);
IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps);
IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps);
IMETHOD friend bool operator==(const FrameVel& a,const FrameVel& b);
IMETHOD friend bool operator!=(const FrameVel& a,const FrameVel& b);
IMETHOD friend bool operator==(const Frame& a,const FrameVel& b);
IMETHOD friend bool operator!=(const Frame& a,const FrameVel& b);
IMETHOD friend bool operator==(const FrameVel& a,const Frame& b);
IMETHOD friend bool operator!=(const FrameVel& a,const Frame& b);
IMETHOD TwistVel Inverse(const TwistVel& arg) const;
IMETHOD TwistVel Inverse(const Twist& arg) const;
IMETHOD TwistVel operator * (const TwistVel& arg) const;
IMETHOD TwistVel operator * (const Twist& arg) const;
};
//very similar to Wrench class.
class TwistVel
// = TITLE
// This class represents a TwistVel. This is a velocity and rotational velocity together
{
public:
VectorVel vel;
VectorVel rot;
public:
// = Constructors
TwistVel():vel(),rot() {};
TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {};
TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {};
TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {};
Twist value() const {
return Twist(vel.value(),rot.value());
}
Twist deriv() const {
return Twist(vel.deriv(),rot.deriv());
}
// = Operators
IMETHOD TwistVel& operator-=(const TwistVel& arg);
IMETHOD TwistVel& operator+=(const TwistVel& arg);
// = External operators
IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs);
IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs);
IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs);
IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs);
IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs);
IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs);
IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs);
IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs);
IMETHOD friend TwistVel operator-(const TwistVel& arg);
IMETHOD friend void SetToZero(TwistVel& v);
// = Zero
static IMETHOD TwistVel Zero();
// = Reverse Sign
IMETHOD void ReverseSign();
// = Change Reference point
IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB);
// Changes the reference point of the TwistVel.
// The VectorVel v_base_AB is expressed in the same base as the TwistVel
// The VectorVel v_base_AB is a VectorVel from the old point to
// the new point.
// Complexity : 6M+6A
// = Equality operators
// do not use operator == because the definition of Equal(.,.) is slightly
// different. It compares whether the 2 arguments are equal in an eps-interval
IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps);
IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps);
IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps);
IMETHOD friend bool operator==(const TwistVel& a,const TwistVel& b);
IMETHOD friend bool operator!=(const TwistVel& a,const TwistVel& b);
IMETHOD friend bool operator==(const Twist& a,const TwistVel& b);
IMETHOD friend bool operator!=(const Twist& a,const TwistVel& b);
IMETHOD friend bool operator==(const TwistVel& a,const Twist& b);
IMETHOD friend bool operator!=(const TwistVel& a,const Twist& b);
// = Conversion to other entities
IMETHOD Twist GetTwist() const;
IMETHOD Twist GetTwistDot() const;
// = Friends
friend class RotationVel;
friend class FrameVel;
};
IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) {
return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt));
}
IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) {
return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt));
}
IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) {
return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt));
}
IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) {
return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt));
}
IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) {
return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt));
}
IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
return FrameVel(
addDelta(a.M,da.rot,dt),
addDelta(a.p,da.vel,dt)
);
}
IMETHOD void random(VectorVel& a) {
random(a.p);
random(a.v);
}
IMETHOD void random(TwistVel& a) {
random(a.vel);
random(a.rot);
}
IMETHOD void random(RotationVel& R) {
random(R.R);
random(R.w);
}
IMETHOD void random(FrameVel& F) {
random(F.M);
random(F.p);
}
IMETHOD void posrandom(VectorVel& a) {
posrandom(a.p);
posrandom(a.v);
}
IMETHOD void posrandom(TwistVel& a) {
posrandom(a.vel);
posrandom(a.rot);
}
IMETHOD void posrandom(RotationVel& R) {
posrandom(R.R);
posrandom(R.w);
}
IMETHOD void posrandom(FrameVel& F) {
posrandom(F.M);
posrandom(F.p);
}
#ifdef KDL_INLINE
#include "framevel.inl"
#endif
} // namespace KDL
template<> struct std::hash<KDL::doubleVel>
{
std::size_t operator()(KDL::doubleVel const& dv) const noexcept
{
size_t seed = 0;
KDL::hash_combine(seed, dv.value());
KDL::hash_combine(seed, dv.deriv());
return seed;
}
};
template<> struct std::hash<KDL::VectorVel>
{
std::size_t operator()(KDL::VectorVel const& vv) const noexcept
{
size_t seed = 0;
KDL::hash_combine(seed, vv.p);
KDL::hash_combine(seed, vv.v);
return seed;
}
};
template<> struct std::hash<KDL::RotationVel>
{
std::size_t operator()(KDL::RotationVel const& rv) const noexcept
{
size_t seed = 0;
KDL::hash_combine(seed, rv.R);
KDL::hash_combine(seed, rv.w);
return seed;
}
};
template<> struct std::hash<KDL::FrameVel>
{
std::size_t operator()(KDL::FrameVel const& fv) const noexcept
{
size_t seed = 0;
KDL::hash_combine(seed, fv.M);
KDL::hash_combine(seed, fv.p);
return seed;
}
};
template<> struct std::hash<KDL::TwistVel>
{
std::size_t operator()(KDL::TwistVel const& tv) const noexcept
{
size_t seed = 0;
KDL::hash_combine(seed, tv.vel);
KDL::hash_combine(seed, tv.rot);
return seed;
}
};
#endif

661
kdl_install/include/kdl/framevel.inl Normal file
View File

@@ -0,0 +1,661 @@
/*****************************************************************************
* \file
* provides inline functions of rframes.h
*
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: rframes.inl,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
* $Name: $
****************************************************************************/
// Methods and operators related to FrameVelVel
// They all delegate most of the work to RotationVelVel and VectorVelVel
FrameVel& FrameVel::operator = (const FrameVel& arg) {
M=arg.M;
p=arg.p;
return *this;
}
FrameVel FrameVel::Identity() {
return FrameVel(RotationVel::Identity(),VectorVel::Zero());
}
FrameVel operator *(const FrameVel& lhs,const FrameVel& rhs)
{
return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
}
FrameVel operator *(const FrameVel& lhs,const Frame& rhs)
{
return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
}
FrameVel operator *(const Frame& lhs,const FrameVel& rhs)
{
return FrameVel(lhs.M*rhs.M , lhs.M*rhs.p+lhs.p );
}
VectorVel FrameVel::operator *(const VectorVel & arg) const
{
return M*arg+p;
}
VectorVel FrameVel::operator *(const Vector & arg) const
{
return M*arg+p;
}
VectorVel FrameVel::Inverse(const VectorVel& arg) const
{
return M.Inverse(arg-p);
}
VectorVel FrameVel::Inverse(const Vector& arg) const
{
return M.Inverse(arg-p);
}
FrameVel FrameVel::Inverse() const
{
return FrameVel(M.Inverse(),-M.Inverse(p));
}
FrameVel& FrameVel::operator = (const Frame& arg) {
M = arg.M;
p = arg.p;
return *this;
}
bool Equal(const FrameVel& r1,const FrameVel& r2,double eps) {
return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
}
bool Equal(const Frame& r1,const FrameVel& r2,double eps) {
return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
}
bool Equal(const FrameVel& r1,const Frame& r2,double eps) {
return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
}
bool operator==(const FrameVel& r1,const FrameVel& r2) {
#ifdef KDL_USE_EQUAL
return Equal(r1, r2);
#else
return (r1.p == r2.p &&
r1.M == r2.M );
#endif
}
bool operator!=(const FrameVel& r1,const FrameVel& r2) {
return !operator==(r1,r2);
}
bool operator==(const Frame& r1,const FrameVel& r2) {
#ifdef KDL_USE_EQUAL
return Equal(r1, r2);
#else
return (r1.p == r2.p &&
r1.M == r2.M );
#endif
}
bool operator!=(const Frame& r1,const FrameVel& r2) {
return !operator==(r1,r2);
}
bool operator==(const FrameVel& r1,const Frame& r2) {
#ifdef KDL_USE_EQUAL
return Equal(r1, r2);
#else
return (r1.p == r2.p &&
r1.M == r2.M );
#endif
}
bool operator!=(const FrameVel& r1,const Frame& r2) {
return !operator==(r1,r2);
}
Frame FrameVel::GetFrame() const {
return Frame(M.R,p.p);
}
Twist FrameVel::GetTwist() const {
return Twist(p.v,M.w);
}
RotationVel operator* (const RotationVel& r1,const RotationVel& r2) {
return RotationVel( r1.R*r2.R, r1.w + r1.R*r2.w );
}
RotationVel operator* (const Rotation& r1,const RotationVel& r2) {
return RotationVel( r1*r2.R, r1*r2.w );
}
RotationVel operator* (const RotationVel& r1,const Rotation& r2) {
return RotationVel( r1.R*r2, r1.w );
}
RotationVel& RotationVel::operator = (const RotationVel& arg) {
R=arg.R;
w=arg.w;
return *this;
}
RotationVel& RotationVel::operator = (const Rotation& arg) {
R=arg;
w=Vector::Zero();
return *this;
}
VectorVel RotationVel::UnitX() const {
return VectorVel(R.UnitX(),w*R.UnitX());
}
VectorVel RotationVel::UnitY() const {
return VectorVel(R.UnitY(),w*R.UnitY());
}
VectorVel RotationVel::UnitZ() const {
return VectorVel(R.UnitZ(),w*R.UnitZ());
}
RotationVel RotationVel::Identity() {
return RotationVel(Rotation::Identity(),Vector::Zero());
}
RotationVel RotationVel::Inverse() const {
return RotationVel(R.Inverse(),-R.Inverse(w));
}
VectorVel RotationVel::Inverse(const VectorVel& arg) const {
Vector tmp=R.Inverse(arg.p);
return VectorVel(tmp,
R.Inverse(arg.v-w*arg.p)
);
}
VectorVel RotationVel::Inverse(const Vector& arg) const {
Vector tmp=R.Inverse(arg);
return VectorVel(tmp,
R.Inverse(-w*arg)
);
}
VectorVel RotationVel::operator*(const VectorVel& arg) const {
Vector tmp=R*arg.p;
return VectorVel(tmp,w*tmp+R*arg.v);
}
VectorVel RotationVel::operator*(const Vector& arg) const {
Vector tmp=R*arg;
return VectorVel(tmp,w*tmp);
}
// = Rotations
// The Rot... static functions give the value of the appropriate rotation matrix back.
// The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
void RotationVel::DoRotX(const doubleVel& angle) {
w+=R*Vector(angle.grad,0,0);
R.DoRotX(angle.t);
}
RotationVel RotationVel::RotX(const doubleVel& angle) {
return RotationVel(Rotation::RotX(angle.t),Vector(angle.grad,0,0));
}
void RotationVel::DoRotY(const doubleVel& angle) {
w+=R*Vector(0,angle.grad,0);
R.DoRotY(angle.t);
}
RotationVel RotationVel::RotY(const doubleVel& angle) {
return RotationVel(Rotation::RotX(angle.t),Vector(0,angle.grad,0));
}
void RotationVel::DoRotZ(const doubleVel& angle) {
w+=R*Vector(0,0,angle.grad);
R.DoRotZ(angle.t);
}
RotationVel RotationVel::RotZ(const doubleVel& angle) {
return RotationVel(Rotation::RotZ(angle.t),Vector(0,0,angle.grad));
}
RotationVel RotationVel::Rot(const Vector& rotvec,const doubleVel& angle)
// rotvec has arbitrary norm
// rotation around a constant vector !
{
Vector v(rotvec);
v.Normalize();
return RotationVel(Rotation::Rot2(v,angle.t),v*angle.grad);
}
RotationVel RotationVel::Rot2(const Vector& rotvec,const doubleVel& angle)
// rotvec is normalized.
{
return RotationVel(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad);
}
VectorVel operator + (const VectorVel& r1,const VectorVel& r2) {
return VectorVel(r1.p+r2.p,r1.v+r2.v);
}
VectorVel operator - (const VectorVel& r1,const VectorVel& r2) {
return VectorVel(r1.p-r2.p,r1.v-r2.v);
}
VectorVel operator + (const VectorVel& r1,const Vector& r2) {
return VectorVel(r1.p+r2,r1.v);
}
VectorVel operator - (const VectorVel& r1,const Vector& r2) {
return VectorVel(r1.p-r2,r1.v);
}
VectorVel operator + (const Vector& r1,const VectorVel& r2) {
return VectorVel(r1+r2.p,r2.v);
}
VectorVel operator - (const Vector& r1,const VectorVel& r2) {
return VectorVel(r1-r2.p,-r2.v);
}
// unary -
VectorVel operator - (const VectorVel& r) {
return VectorVel(-r.p,-r.v);
}
void SetToZero(VectorVel& v){
SetToZero(v.p);
SetToZero(v.v);
}
// cross prod.
VectorVel operator * (const VectorVel& r1,const VectorVel& r2) {
return VectorVel(r1.p*r2.p, r1.p*r2.v+r1.v*r2.p);
}
VectorVel operator * (const VectorVel& r1,const Vector& r2) {
return VectorVel(r1.p*r2, r1.v*r2);
}
VectorVel operator * (const Vector& r1,const VectorVel& r2) {
return VectorVel(r1*r2.p, r1*r2.v);
}
// scalar mult.
VectorVel operator * (double r1,const VectorVel& r2) {
return VectorVel(r1*r2.p, r1*r2.v);
}
VectorVel operator * (const VectorVel& r1,double r2) {
return VectorVel(r1.p*r2, r1.v*r2);
}
VectorVel operator * (const doubleVel& r1,const VectorVel& r2) {
return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
}
VectorVel operator * (const VectorVel& r2,const doubleVel& r1) {
return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
}
VectorVel operator / (const VectorVel& r1,double r2) {
return VectorVel(r1.p/r2, r1.v/r2);
}
VectorVel operator / (const VectorVel& r2,const doubleVel& r1) {
return VectorVel(r2.p/r1.t, r2.v/r1.t - r2.p*r1.grad/r1.t/r1.t);
}
VectorVel operator*(const Rotation& R,const VectorVel& x) {
return VectorVel(R*x.p,R*x.v);
}
VectorVel& VectorVel::operator = (const VectorVel& arg) {
p=arg.p;
v=arg.v;
return *this;
}
VectorVel& VectorVel::operator = (const Vector& arg) {
p=arg;
v=Vector::Zero();
return *this;
}
VectorVel& VectorVel::operator += (const VectorVel& arg) {
p+=arg.p;
v+=arg.v;
return *this;
}
VectorVel& VectorVel::operator -= (const VectorVel& arg) {
p-=arg.p;
v-=arg.v;
return *this;
}
VectorVel VectorVel::Zero() {
return VectorVel(Vector::Zero(),Vector::Zero());
}
void VectorVel::ReverseSign() {
p.ReverseSign();
v.ReverseSign();
}
doubleVel VectorVel::Norm(double eps) const {
double n = p.Norm(eps);
if (n < eps) // Setting norm of p and v to 0 in case norm of p is smaller than eps
return doubleVel(0, 0);
return doubleVel(n, dot(p,v)/n);
}
bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) {
return (Equal(r1.p,r2.p,eps) && Equal(r1.v,r2.v,eps));
}
bool Equal(const Vector& r1,const VectorVel& r2,double eps) {
return (Equal(r1,r2.p,eps) && Equal(Vector::Zero(),r2.v,eps));
}
bool Equal(const VectorVel& r1,const Vector& r2,double eps) {
return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps));
}
bool operator==(const VectorVel& r1,const VectorVel& r2) {
#ifdef KDL_USE_EQUAL
return Equal(r1, r2);
#else
return (r1.p == r2.p &&
r1.v == r2.v );
#endif
}
bool operator!=(const VectorVel& r1,const VectorVel& r2) {
return !operator==(r1,r2);
}
bool operator==(const Vector& r1,const VectorVel& r2) {
#ifdef KDL_USE_EQUAL
return Equal(r1, r2);
#else
return (r1 == r2.p &&
Vector::Zero() == r2.v);
#endif
}
bool operator!=(const Vector& r1,const VectorVel& r2) {
return !operator==(r1,r2);
}
bool operator==(const VectorVel& r1,const Vector& r2) {
#ifdef KDL_USE_EQUAL
return Equal(r1, r2);
#else
return (r1.p == r2 &&
r1.v == Vector::Zero() );
#endif
}
bool operator!=(const VectorVel& r1,const Vector& r2) {
return !operator==(r1,r2);
}
bool Equal(const RotationVel& r1,const RotationVel& r2,double eps) {
return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps));
}
bool Equal(const Rotation& r1,const RotationVel& r2,double eps) {
return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps));
}
bool Equal(const RotationVel& r1,const Rotation& r2,double eps) {
return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps));
}
bool operator==(const RotationVel& r1,const RotationVel& r2) {
#ifdef KDL_USE_EQUAL
return Equal(r1, r2);
#else
return (r1.w == r2.w &&
r1.R == r2.R );
#endif
}
bool operator!=(const RotationVel& r1,const RotationVel& r2) {
return !operator==(r1,r2);
}
bool operator==(const Rotation& r1,const RotationVel& r2) {
#ifdef KDL_USE_EQUAL
return Equal(r1, r2);
#else
return (Vector::Zero() == r2.w &&
r1 == r2.R);
#endif
}
bool operator!=(const Rotation& r1,const RotationVel& r2) {
return !operator==(r1,r2);
}
bool operator==(const RotationVel& r1,const Rotation& r2) {
#ifdef KDL_USE_EQUAL
return Equal(r1, r2);
#else
return (r1.w == Vector::Zero() &&
r1.R == r2);
#endif
}
bool operator!=(const RotationVel& r1,const Rotation& r2) {
return !operator==(r1,r2);
}
bool Equal(const TwistVel& a,const TwistVel& b,double eps) {
return (Equal(a.rot,b.rot,eps)&&
Equal(a.vel,b.vel,eps) );
}
bool Equal(const Twist& a,const TwistVel& b,double eps) {
return (Equal(a.rot,b.rot,eps)&&
Equal(a.vel,b.vel,eps) );
}
bool Equal(const TwistVel& a,const Twist& b,double eps) {
return (Equal(a.rot,b.rot,eps)&&
Equal(a.vel,b.vel,eps) );
}
bool operator==(const TwistVel& a,const TwistVel& b) {
#ifdef KDL_USE_EQUAL
return Equal(a, b);
#else
return (a.rot == b.rot &&
a.vel == b.vel );
#endif
}
bool operator!=(const TwistVel& a,const TwistVel& b) {
return !operator==(a,b);
}
bool operator==(const Twist& a,const TwistVel& b) {
#ifdef KDL_USE_EQUAL
return Equal(a, b);
#else
return (a.rot == b.rot &&
a.vel == b.vel );
#endif
}
bool operator!=(const Twist& r1,const TwistVel& r2) {
return !operator==(r1,r2);
}
bool operator==(const TwistVel& r1,const Twist& r2) {
#ifdef KDL_USE_EQUAL
return Equal(r1, r2);
#else
return (a.rot == b.rot &&
a.vel == b.vel );
#endif
}
bool operator!=(const TwistVel& r1,const Twist& r2) {
return !operator==(r1,r2);
}
IMETHOD doubleVel dot(const VectorVel& lhs,const VectorVel& rhs) {
return doubleVel(dot(lhs.p,rhs.p),dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p));
}
IMETHOD doubleVel dot(const VectorVel& lhs,const Vector& rhs) {
return doubleVel(dot(lhs.p,rhs),dot(lhs.v,rhs));
}
IMETHOD doubleVel dot(const Vector& lhs,const VectorVel& rhs) {
return doubleVel(dot(lhs,rhs.p),dot(lhs,rhs.v));
}
TwistVel TwistVel::Zero()
{
return TwistVel(VectorVel::Zero(),VectorVel::Zero());
}
void TwistVel::ReverseSign()
{
vel.ReverseSign();
rot.ReverseSign();
}
TwistVel TwistVel::RefPoint(const VectorVel& v_base_AB)
// Changes the reference point of the TwistVel.
// The VectorVel v_base_AB is expressed in the same base as the TwistVel
// The VectorVel v_base_AB is a VectorVel from the old point to
// the new point.
// Complexity : 6M+6A
{
return TwistVel(this->vel+this->rot*v_base_AB,this->rot);
}
TwistVel& TwistVel::operator-=(const TwistVel& arg)
{
vel-=arg.vel;
rot -=arg.rot;
return *this;
}
TwistVel& TwistVel::operator+=(const TwistVel& arg)
{
vel+=arg.vel;
rot +=arg.rot;
return *this;
}
TwistVel operator*(const TwistVel& lhs,double rhs)
{
return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
}
TwistVel operator*(double lhs,const TwistVel& rhs)
{
return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
}
TwistVel operator/(const TwistVel& lhs,double rhs)
{
return TwistVel(lhs.vel/rhs,lhs.rot/rhs);
}
TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs)
{
return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
}
TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs)
{
return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
}
TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs)
{
return TwistVel(lhs.vel/rhs,lhs.rot/rhs);
}
// addition of TwistVel's
TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs)
{
return TwistVel(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
}
TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs)
{
return TwistVel(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
}
// unary -
TwistVel operator-(const TwistVel& arg)
{
return TwistVel(-arg.vel,-arg.rot);
}
void SetToZero(TwistVel& v)
{
SetToZero(v.vel);
SetToZero(v.rot);
}
TwistVel RotationVel::Inverse(const TwistVel& arg) const
{
return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
}
TwistVel RotationVel::operator * (const TwistVel& arg) const
{
return TwistVel((*this)*arg.vel,(*this)*arg.rot);
}
TwistVel RotationVel::Inverse(const Twist& arg) const
{
return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
}
TwistVel RotationVel::operator * (const Twist& arg) const
{
return TwistVel((*this)*arg.vel,(*this)*arg.rot);
}
TwistVel FrameVel::operator * (const TwistVel& arg) const
{
TwistVel tmp;
tmp.rot = M*arg.rot;
tmp.vel = M*arg.vel+p*tmp.rot;
return tmp;
}
TwistVel FrameVel::operator * (const Twist& arg) const
{
TwistVel tmp;
tmp.rot = M*arg.rot;
tmp.vel = M*arg.vel+p*tmp.rot;
return tmp;
}
TwistVel FrameVel::Inverse(const TwistVel& arg) const
{
TwistVel tmp;
tmp.rot = M.Inverse(arg.rot);
tmp.vel = M.Inverse(arg.vel-p*arg.rot);
return tmp;
}
TwistVel FrameVel::Inverse(const Twist& arg) const
{
TwistVel tmp;
tmp.rot = M.Inverse(arg.rot);
tmp.vel = M.Inverse(arg.vel-p*arg.rot);
return tmp;
}
Twist TwistVel::GetTwist() const {
return Twist(vel.p,rot.p);
}
Twist TwistVel::GetTwistDot() const {
return Twist(vel.v,rot.v);
}

55
kdl_install/include/kdl/framevel_io.hpp Normal file
View File

@@ -0,0 +1,55 @@
/*****************************************************************************
* \file
* provides I/O operations on FrameVels classes
*
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V2
*
* \par History
* - $log$
*
* \par Release
* $Id: rframes_io.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_FRAMESVEL_IO
#define KDL_FRAMESVEL_IO
#include "utilities/utility_io.h"
#include "utilities/rall1d_io.h"
#include "frames_io.hpp"
namespace KDL {
// Output...
inline std::ostream& operator << (std::ostream& os,const VectorVel& r) {
os << "{" << r.p << "," << r.v << "}" << std::endl;
return os;
}
inline std::ostream& operator << (std::ostream& os,const RotationVel& r) {
os << "{" << std::endl << r.R << "," <<std::endl << r.w << std::endl << "}" << std::endl;
return os;
}
inline std::ostream& operator << (std::ostream& os,const FrameVel& r) {
os << "{" << std::endl << r.M << "," << std::endl << r.p << std::endl << "}" << std::endl;
return os;
}
inline std::ostream& operator << (std::ostream& os,const TwistVel& r) {
os << "{" << std::endl << r.vel << "," << std::endl << r.rot << std::endl << "}" << std::endl;
return os;
}
} // namespace Frame
#endif

88
kdl_install/include/kdl/jacobian.hpp Normal file
View File

@@ -0,0 +1,88 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_JACOBIAN_HPP
#define KDL_JACOBIAN_HPP
#include "frames.hpp"
#include <Eigen/Core>
namespace KDL
{
// Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4)
class Jacobian;
bool Equal(const Jacobian& a,const Jacobian& b,double eps=epsilon);
void SetToZero(Jacobian& jac);
class Jacobian
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::Matrix<double,6,Eigen::Dynamic> data;
Jacobian();
explicit Jacobian(unsigned int nr_of_columns);
Jacobian(const Jacobian& arg);
///Allocates memory for new size (can break realtime behavior)
void resize(unsigned int newNrOfColumns);
///Allocates memory if size of this and argument is different
Jacobian& operator=(const Jacobian& arg);
bool operator ==(const Jacobian& arg)const;
bool operator !=(const Jacobian& arg)const;
friend bool Equal(const Jacobian& a,const Jacobian& b,double eps);
~Jacobian();
double operator()(unsigned int i,unsigned int j)const;
double& operator()(unsigned int i,unsigned int j);
unsigned int rows()const;
unsigned int columns()const;
friend void SetToZero(Jacobian& jac);
friend bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest);
friend bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest);
friend bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest);
Twist getColumn(unsigned int i) const;
void setColumn(unsigned int i,const Twist& t);
void changeRefPoint(const Vector& base_AB);
void changeBase(const Rotation& rot);
void changeRefFrame(const Frame& frame);
};
bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest);
bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest);
bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest);
}
#endif

227
kdl_install/include/kdl/jntarray.hpp Normal file
View File

@@ -0,0 +1,227 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_JNTARRAY_HPP
#define KDL_JNTARRAY_HPP
#include "frames.hpp"
#include "jacobian.hpp"
#include <Eigen/Core>
namespace KDL
{
/**
* @brief This class represents an fixed size array containing
* joint values of a KDL::Chain.
*
* \warning An object constructed with the default constructor provides
* a valid, but inert, object. Many of the member functions will do
* the correct thing and have no affect on this object, but some
* member functions can _NOT_ deal with an inert/empty object. These
* functions will assert() and exit the program instead. The intended use
* case for the default constructor (in an RTT/OCL setting) is outlined in
* code below - the default constructor plus the resize() function allow
* use of JntArray objects whose size is set within a configureHook() call
* (typically based on a size determined from a property).
\code
class MyTask : public RTT::TaskContext
{
JntArray j;
MyTask()
{} // invokes j's default constructor
bool configureHook()
{
unsigned int size = some_property.rvalue();
j.resize(size)
...
}
void updateHook()
{
** use j here
}
};
\endcode
*/
class JntArray
{
public:
Eigen::VectorXd data;
/** Construct with _no_ data array
* @post NULL == data
* @post 0 == rows()
* @warning use of an object constructed like this, without
* a resize() first, may result in program exit! See class
* documentation.
*/
JntArray();
/**
* Constructor of the joint array
*
* @param size size of the array, this cannot be changed
* afterwards.
* @pre 0 < size
* @post NULL != data
* @post 0 < rows()
* @post all elements in data have 0 value
*/
explicit JntArray(unsigned int size);
/** Copy constructor
* @note Will correctly copy an empty object
*/
JntArray(const JntArray& arg);
~JntArray();
/** Resize the array
* @warning This causes a dynamic allocation (and potentially
* also a dynamic deallocation). This _will_ negatively affect
* real-time performance!
*
* @post newSize == rows()
* @post NULL != data
* @post all elements in data have 0 value
*/
void resize(unsigned int newSize);
JntArray& operator = ( const JntArray& arg);
/**
* get_item operator for the joint array, if a second value is
* given it should be zero, since a JntArray resembles a column.
*
*
* @return the joint value at position i, starting from 0
* @pre 0 != size (ie non-default constructor or resize() called)
*/
double operator()(unsigned int i,unsigned int j=0)const;
/**
* set_item operator, again if a second value is given it
*should be zero.
*
* @return reference to the joint value at position i,starting
*from zero.
* @pre 0 != size (ie non-default constructor or resize() called)
*/
double& operator()(unsigned int i,unsigned int j=0);
/**
* Returns the number of rows (size) of the array
*
*/
unsigned int rows()const;
/**
* Returns the number of columns of the array, always 1.
*/
unsigned int columns()const;
friend void Add(const JntArray& src1,const JntArray& src2,JntArray& dest);
friend void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest);
friend void Multiply(const JntArray& src,const double& factor,JntArray& dest);
friend void Divide(const JntArray& src,const double& factor,JntArray& dest);
friend void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest);
friend void SetToZero(JntArray& array);
friend bool Equal(const JntArray& src1,const JntArray& src2,double eps);
friend bool operator==(const JntArray& src1,const JntArray& src2);
//friend bool operator!=(const JntArray& src1,const JntArray& src2);
};
bool operator==(const JntArray& src1,const JntArray& src2);
//bool operator!=(const JntArray& src1,const JntArray& src2);
/**
* Function to add two joint arrays, all the arguments must
* have the same size: A + B = C. This function is
* aliasing-safe, A or B can be the same array as C.
*
* @param src1 A
* @param src2 B
* @param dest C
*/
void Add(const JntArray& src1,const JntArray& src2,JntArray& dest);
/**
* Function to subtract two joint arrays, all the arguments must
* have the same size: A - B = C. This function is
* aliasing-safe, A or B can be the same array as C.
*
* @param src1 A
* @param src2 B
* @param dest C
*/
void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest);
/**
* Function to multiply all the array values with a scalar
* factor: A*b=C. This function is aliasing-safe, A can be the
* same array as C.
*
* @param src A
* @param factor b
* @param dest C
*/
void Multiply(const JntArray& src,const double& factor,JntArray& dest);
/**
* Function to divide all the array values with a scalar
* factor: A/b=C. This function is aliasing-safe, A can be the
* same array as C.
*
* @param src A
* @param factor b
* @param dest C
*/
void Divide(const JntArray& src,const double& factor,JntArray& dest);
/**
* Function to multiply a KDL::Jacobian with a KDL::JntArray
* to get a KDL::Twist, it should not be used to calculate the
* forward velocity kinematics, the solver classes are built
* for this purpose.
* J*q = t
*
* @param jac J
* @param src q
* @param dest t
* @post dest==Twist::Zero() if 0==src.rows() (ie src is empty)
*/
void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest);
/**
* Function to set all the values of the array to 0
*
* @param array
*/
void SetToZero(JntArray& array);
/**
* Function to check if two arrays are the same with a
*precision of eps
*
* @param src1
* @param src2
* @param eps default: epsilon
* @return true if each element of src1 is within eps of the same
* element in src2, or if both src1 and src2 have no data (ie 0==rows())
*/
bool Equal(const JntArray& src1,const JntArray& src2,double eps=epsilon);
}
#endif

87
kdl_install/include/kdl/jntarrayacc.hpp Normal file
View File

@@ -0,0 +1,87 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_JNTARRAYACC_HPP
#define KDL_JNTARRAYACC_HPP
#include "utilities/utility.h"
#include "jntarray.hpp"
#include "jntarrayvel.hpp"
#include "frameacc.hpp"
namespace KDL
{
// Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4)
class JntArrayAcc;
bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps=epsilon);
void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
void SetToZero(JntArrayAcc& array);
class JntArrayAcc
{
public:
JntArray q;
JntArray qdot;
JntArray qdotdot;
public:
JntArrayAcc(){};
explicit JntArrayAcc(unsigned int size);
JntArrayAcc(const JntArray& q,const JntArray& qdot,const JntArray& qdotdot);
JntArrayAcc(const JntArray& q,const JntArray& qdot);
explicit JntArrayAcc(const JntArray& q);
void resize(unsigned int newSize);
JntArray value()const;
JntArray deriv()const;
JntArray dderiv()const;
friend void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
friend void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
friend void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
friend void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
friend void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
friend void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
friend void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
friend void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
friend void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
friend void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
friend void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
friend void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
friend void SetToZero(JntArrayAcc& array);
friend bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps);
};
}
#endif

76
kdl_install/include/kdl/jntarrayvel.hpp Normal file
View File

@@ -0,0 +1,76 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_JNTARRAYVEL_HPP
#define KDL_JNTARRAYVEL_HPP
#include "utilities/utility.h"
#include "jntarray.hpp"
#include "framevel.hpp"
namespace KDL
{
// Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4)
class JntArrayVel;
bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps=epsilon);
void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest);
void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest);
void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest);
void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest);
void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest);
void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest);
void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest);
void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest);
void SetToZero(JntArrayVel& array);
class JntArrayVel
{
public:
JntArray q;
JntArray qdot;
public:
JntArrayVel(){};
explicit JntArrayVel(unsigned int size);
JntArrayVel(const JntArray& q,const JntArray& qdot);
explicit JntArrayVel(const JntArray& q);
void resize(unsigned int newSize);
JntArray value()const;
JntArray deriv()const;
friend void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest);
friend void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest);
friend void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest);
friend void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest);
friend void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest);
friend void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest);
friend void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest);
friend void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest);
friend void SetToZero(JntArrayVel& array);
friend bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps);
};
}
#endif

232
kdl_install/include/kdl/jntspaceinertiamatrix.hpp Normal file
View File

@@ -0,0 +1,232 @@
// Copyright (C) 2009 Dominick Vanthienen <dominick dot vanthienen at intermodalics dot eu>
// Version: 1.0
// Author: Dominick Vanthienen <dominick dot vanthienen at intermodalics dot eu>
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_JNTSPACEINERTIAMATRIX_HPP
#define KDL_JNTSPACEINERTIAMATRIX_HPP
#include "frames.hpp"
#include "jacobian.hpp"
#include "jntarray.hpp"
#include <Eigen/Core>
namespace KDL
{
/**
* @brief This class represents an fixed size matrix containing
* the Joint-Space Inertia Matrix of a KDL::Chain.
*
* \warning An object constructed with the default constructor provides
* a valid, but inert, object. Many of the member functions will do
* the correct thing and have no affect on this object, but some
* member functions can _NOT_ deal with an inert/empty object. These
* functions will assert() and exit the program instead. The intended use
* case for the default constructor (in an RTT/OCL setting) is outlined in
* code below - the default constructor plus the resize() function allow
* use of JntSpaceInertiaMatrix objects whose size is set within a configureHook() call
* (typically based on a size determined from a property).
\code
class MyTask : public RTT::TaskContext
{
JntSpaceInertiaMatrix j;
MyTask()
{} // invokes j's default constructor
bool configureHook()
{
unsigned int size = some_property.rvalue();
j.resize(size)
...
}
void updateHook()
{
** use j here
}
};
/endcode
*/
class JntSpaceInertiaMatrix
{
public:
Eigen::MatrixXd data;
/** Construct with _no_ data array
* @post NULL == data
* @post 0 == rows()
* @warning use of an object constructed like this, without
* a resize() first, may result in program exit! See class
* documentation.
*/
JntSpaceInertiaMatrix();
/**
* Constructor of the Joint-Space Inertia Matrix
*
* @param size of the matrix, this cannot be changed
* afterwards. Size rows and size columns.
* @pre 0 < size
* @post NULL != data
* @post 0 < rows()
* @post all elements in data have 0 value
*/
explicit JntSpaceInertiaMatrix(int size);
/** Copy constructor
* @note Will correctly copy an empty object
*/
JntSpaceInertiaMatrix(const JntSpaceInertiaMatrix& arg);
~JntSpaceInertiaMatrix();
/** Resize the array
* @warning This causes a dynamic allocation (and potentially
* also a dynamic deallocation). This _will_ negatively affect
* real-time performance!
*
* @post newSize == rows()
* @post NULL != data
* @post all elements in data have 0 value
*/
void resize(unsigned int newSize);
JntSpaceInertiaMatrix& operator = ( const JntSpaceInertiaMatrix& arg);
/**
* get_item operator for the joint matrix
*
*
* @return the joint value at position i, starting from 0
* @pre 0 != size (ie non-default constructor or resize() called)
*/
double operator()(unsigned int i,unsigned int j)const;
/**
* set_item operator
*
* @return reference to the joint value at position i,starting
*from zero.
* @pre 0 != size (ie non-default constructor or resize() called)
*/
double& operator()(unsigned int i,unsigned int j);
/**
* Returns the number of rows and columns of the matrix
*
*/
unsigned int rows()const;
/**
* Returns the number of columns of the matrix.
*/
unsigned int columns()const;
friend void Add(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest);
friend void Subtract(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest);
friend void Multiply(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest);
friend void Divide(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest);
friend void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest);
friend void SetToZero(JntSpaceInertiaMatrix& matrix);
friend bool Equal(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,double eps);
friend bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2);
//friend bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2);
};
bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2);
//bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2);
/**
* Function to add two joint matrix, all the arguments must
* have the same size: A + B = C. This function is
* aliasing-safe, A or B can be the same array as C.
*
* @param src1 A
* @param src2 B
* @param dest C
*/
void Add(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest);
/**
* Function to subtract two joint matrix, all the arguments must
* have the same size: A - B = C. This function is
* aliasing-safe, A or B can be the same array as C.
*
* @param src1 A
* @param src2 B
* @param dest C
*/
void Subtract(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest);
/**
* Function to multiply all the array values with a scalar
* factor: A*b=C. This function is aliasing-safe, A can be the
* same array as C.
*
* @param src A
* @param factor b
* @param dest C
*/
void Multiply(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest);
/**
* Function to divide all the array values with a scalar
* factor: A/b=C. This function is aliasing-safe, A can be the
* same array as C.
*
* @param src A
* @param factor b
* @param dest C
*/
void Divide(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest);
/**
* Function to multiply a KDL::Jacobian with a KDL::JntSpaceInertiaMatrix
* to get a KDL::Twist, it should not be used to calculate the
* forward velocity kinematics, the solver classes are built
* for this purpose.
* J*q = t
*
* @param jac J
* @param src q
* @param dest t
* @post dest==Twist::Zero() if 0==src.rows() (ie src is empty)
*/
void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest);
/**
* Function to set all the values of the array to 0
*
* @param array
*/
void SetToZero(JntSpaceInertiaMatrix& matrix);
/**
* Function to check if two matrices are the same with a
*precision of eps
*
* @param src1
* @param src2
* @param eps default: epsilon
* @return true if each element of src1 is within eps of the same
* element in src2, or if both src1 and src2 have no data (ie 0==rows())
*/
bool Equal(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,double eps=epsilon);
bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2);
}
#endif

290
kdl_install/include/kdl/joint.hpp Normal file
View File

@@ -0,0 +1,290 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at intermodalics dot eu>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_JOINT_HPP
#define KDL_JOINT_HPP
#include "frames.hpp"
#include <string>
#include <exception>
namespace KDL {
/**
* \brief This class encapsulates a simple joint, that is with one
* parameterized degree of freedom and with scalar dynamic properties.
*
* A simple joint is described by the following properties :
* - scale: ratio between motion input and motion output
* - offset: between the "physical" and the "logical" zero position.
* - type: revolute or translational, along one of the basic frame axes
* - inertia, stiffness and damping: scalars representing the physical
* effects along/about the joint axis only.
*
* @ingroup KinematicFamily
*/
class Joint {
public:
typedef enum { RotAxis,RotX,RotY,RotZ,TransAxis,TransX,TransY,TransZ,Fixed,None=Fixed} JointType;
/**
* Constructor of a joint.
*
* @param name of the joint
* @param type type of the joint, default: Joint::Fixed
* @param scale scale between joint input and actual geometric
* movement, default: 1
* @param offset offset between joint input and actual
* geometric input, default: 0
* @param inertia 1D inertia along the joint axis, default: 0
* @param damping 1D damping along the joint axis, default: 0
* @param stiffness 1D stiffness along the joint axis,
* default: 0
*/
explicit Joint(const std::string& name, const JointType& type=Fixed,const double& scale=1,const double& offset=0,
const double& inertia=0,const double& damping=0,const double& stiffness=0);
/**
* Constructor of a joint.
*
* @param type type of the joint, default: Joint::Fixed
* @param scale scale between joint input and actual geometric
* movement, default: 1
* @param offset offset between joint input and actual
* geometric input, default: 0
* @param inertia 1D inertia along the joint axis, default: 0
* @param damping 1D damping along the joint axis, default: 0
* @param stiffness 1D stiffness along the joint axis,
* default: 0
*/
explicit Joint(const JointType& type=Fixed,const double& scale=1,const double& offset=0,
const double& inertia=0,const double& damping=0,const double& stiffness=0);
/**
* Constructor of a joint.
*
* @param name of the joint
* @param origin the origin of the joint
* @param axis the axis of the joint
* @param scale scale between joint input and actual geometric
* movement, default: 1
* @param offset offset between joint input and actual
* geometric input, default: 0
* @param inertia 1D inertia along the joint axis, default: 0
* @param damping 1D damping along the joint axis, default: 0
* @param stiffness 1D stiffness along the joint axis,
* default: 0
*/
Joint(const std::string& name, const Vector& _origin, const Vector& _axis, const JointType& type, const double& _scale=1, const double& _offset=0,
const double& _inertia=0, const double& _damping=0, const double& _stiffness=0);
/**
* Constructor of a joint.
*
* @param origin the origin of the joint
* @param axis the axis of the joint
* @param scale scale between joint input and actual geometric
* movement, default: 1
* @param offset offset between joint input and actual
* geometric input, default: 0
* @param inertia 1D inertia along the joint axis, default: 0
* @param damping 1D damping along the joint axis, default: 0
* @param stiffness 1D stiffness along the joint axis,
* default: 0
*/
Joint(const Vector& _origin, const Vector& _axis, const JointType& type, const double& _scale=1, const double& _offset=0,
const double& _inertia=0, const double& _damping=0, const double& _stiffness=0);
/**
* Request the 6D-pose between the beginning and the end of
* the joint at joint position q
*
* @param q the 1D joint position
*
* @return the resulting 6D-pose
*/
Frame pose(const double& q)const;
/**
* Request the resulting 6D-velocity with a joint velocity qdot
*
* @param qdot the 1D joint velocity
*
* @return the resulting 6D-velocity
*/
Twist twist(const double& qdot)const;
/**
* Request the Vector corresponding to the axis of a revolute joint.
*
* @return Vector. e.g (1,0,0) for RotX etc.
*/
Vector JointAxis() const;
/**
* Request the Vector corresponding to the origin of a revolute joint.
*
* @return Vector
*/
Vector JointOrigin() const;
/**
* Request the name of the joint
*
*
* @return const reference to the name of the joint
*/
const std::string& getName()const
{
return name;
}
/**
* Request the type of the joint.
*
* @return const reference to the type
*/
const JointType& getType() const
{
return type;
};
/**
* Request the stringified type of the joint.
*
* @return const string
*/
const std::string getTypeName() const
{
switch (type) {
case RotAxis:
return "RotAxis";
case TransAxis:
return "TransAxis";
case RotX:
return "RotX";
case RotY:
return "RotY";
case RotZ:
return "RotZ";
case TransX:
return "TransX";
case TransY:
return "TransY";
case TransZ:
return "TransZ";
case Fixed:
return "Fixed";
default:
return "Fixed";
}
};
/**
* Request the scale of the joint.
*
* @return const reference to the scale of the joint
*/
const double& getScale() const
{
return scale;
}
/**
* Request the offset of the joint.
*
* @return const reference to the offset of the joint
*/
const double& getOffset() const
{
return offset;
}
/**
* Request the inertia of the joint.
*
* @return const reference to the inertia of the joint
*/
const double& getInertia() const
{
return inertia;
};
/**
* Request the damping of the joint.
*
* @return const reference to the damping of the joint
*/
const double& getDamping() const
{
return damping;
};
/**
* Request the stiffness of the joint.
*
* @return const reference to the stiffness of the joint
*/
const double& getStiffness() const
{
return stiffness;
};
/**
* Request the axis of the joint.
*
* @return const reference to the axis of the joint
*/
const Vector& getAxis() const
{
return axis;
}
/**
* Request the origin of the joint.
*
* @return const reference to the origin of the joint
*/
const Vector& getOrigin() const
{
return origin;
}
virtual ~Joint();
private:
std::string name;
Joint::JointType type;
double scale;
double offset;
double inertia;
double damping;
double stiffness;
// variables for RotAxis joint
Vector axis, origin;
mutable Frame joint_pose; // Deprecated, but keeping for ABI compatibility
mutable double q_previous; // Deprecated, but keeping for ABI compatibility
class joint_type_exception: public std::exception{
virtual const char* what() const throw(){
return "Joint Type excption";}
} joint_type_ex;
};
} // end of namespace KDL
#endif

129
kdl_install/include/kdl/kdl.hpp Normal file
View File

@@ -0,0 +1,129 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Copyright (C) 2011 Erwin Aertbelien <Erwin dot Aertbelien at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
/**
* \mainpage KDL
*
* This is the API reference of the
* <a href="http://www.orocos.org/kdl">Kinematics and Dynamics
* Library</a> (KDL), a sub-project of <a
* href="http://www.orocos.org">Orocos</a>, but that can be used
* independently of Orocos. KDL offers different kinds of
* functionality, grouped in the following Modules:
* - \subpage geomprim
* - \ref KinematicFamily : functionality to build kinematic chains and access their kinematic and dynamic properties, such as e.g. Forward and Inverse kinematics and dynamics.
* - \ref Motion : functionality to specify motion trajectories of frames and kinematic chains, such as e.g. Trapezoidal Velocity profiles.
* - \ref KDLTK : the interface code to integrate KDL into the Orocos <a href="http://www.orocos.org/rtt/">Real-Time Toolkit</a> (RTT).
*
*
**/
/**
* \page geomprim Geometric Primitives
* \section Introduction
* Geometric primitives are represented by the following classes.
* - KDL::Vector
* - KDL::Rotation
* - KDL::Frame
* - KDL::Twist
* - KDL::Wrench
*
* \par Twist and Wrench transformations
* 3 different types of transformations do exist for the twists
* and wrenches.
*
* \verbatim
* 1) Frame * Twist or Frame * Wrench :
* this transforms both the velocity/force reference point
* and the basis to which the twist/wrench are expressed.
* 2) Rotation * Twist or Rotation * Wrench :
* this transforms the basis to which the twist/wrench are
* expressed, but leaves the reference point intact.
* 3) Twist.RefPoint(v_base_AB) or Wrench.RefPoint(v_base_AB)
* this transforms only the reference point. v is expressed
* in the same base as the twist/wrench and points from the
* old reference point to the new reference point.
* \endverbatim
*
*\warning
* Efficienty can be improved by writing p2 = A*(B*(C*p1))) instead of
* p2=A*B*C*p1
*
* \par PROPOSED NAMING CONVENTION FOR FRAME-like OBJECTS
*
* \verbatim
* A naming convention of objects of the type defined in this file :
* (1) Frame : F...
* Rotation : R ...
* (2) Twist : T ...
* Wrench : W ...
* Vector : V ...
* This prefix is followed by :
* for category (1) :
* F_A_B : w.r.t. frame A, frame B expressed
* ( each column of F_A_B corresponds to an axis of B,
* expressed w.r.t. frame A )
* in mathematical convention :
* A
* F_A_B == F
* B
*
* for category (2) :
* V_B : a vector expressed w.r.t. frame B
*
* This can also be prepended by a name :
* e.g. : temporaryV_B
*
* With this convention one can write :
*
* F_A_B = F_B_A.Inverse();
* F_A_C = F_A_B * F_B_C;
* V_B = F_B_C * V_C; // both translation and rotation
* V_B = R_B_C * V_C; // only rotation
* \endverbatim
*
* \par CONVENTIONS FOR WHEN USED WITH ROBOTS :
*
* \verbatim
* world : represents the frame ([1 0 0,0 1 0,0 0 1],[0 0 0]')
* mp : represents mounting plate of a robot
* (i.e. everything before MP is constructed by robot manufacturer
* everything after MP is tool )
* tf : represents task frame of a robot
* (i.e. frame in which motion and force control is expressed)
* sf : represents sensor frame of a robot
* (i.e. frame at which the forces measured by the force sensor
* are expressed )
*
* Frame F_world_mp=...;
* Frame F_mp_sf(..)
* Frame F_mp_tf(,.)
*
* Wrench are measured in sensor frame SF, so one could write :
* Wrench_tf = F_mp_tf.Inverse()* ( F_mp_sf * Wrench_sf );
* \endverbatim
*
* \par CONVENTIONS REGARDING UNITS :
* Typically we use the standard S.I. units: N, m, sec.
*
*/

52
kdl_install/include/kdl/kinfam.hpp Normal file
View File

@@ -0,0 +1,52 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
/**
* @defgroup KinematicFamily Kinematic Families
* @brief All classes to support kinematic families.
*
* The Kinematic Families classes range from the basic building blocks
* (KDL::Joint and KDL::Segment) and their interconnected kinematic
* structures (KDL::Chain and KDL::Tree), to the solver
* algorithms for the kinematics and dynamics of particular kinematic
* families.
*
* A <em>kinematic family</em> is a set of kinematic structures that have
* similar properties, such as the same interconnection topology, the same
* numerical or analytical solver algorithms, etc. Different members of the
* same kinematic family differ only by the concrete values of their
* kinematic and dynamic properties (link lengths, mass, etc.).
*
* Each kinematic structure is built from one or more Segments
* (KDL::Segment). A KDL::Chain is a <strong>serial</strong> connection of
* these segments; a KDL:Tree is a <strong>tree-structured</strong>
* interconnection; and a KDL:Graph is a kinematic structure with a
* <strong>general graph</strong> topology. (The current implementation
* supports only KDL::Chain.)
*
* A KDL::Segment contains a KDL::Joint and an offset frame ("link length",
* defined by a KDL::Frame), that represents the geometric pose
* between the KDL::Joint on the previous segment and its own KDL::Joint.
*
* A list of all the classes is available on the modules page: \ref KinematicFamily
*
*
*/

81
kdl_install/include/kdl/kinfam_io.hpp Normal file
View File

@@ -0,0 +1,81 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_KINFAM_IO_HPP
#define KDL_KINFAM_IO_HPP
#include <iostream>
#include <fstream>
#include "joint.hpp"
#include "segment.hpp"
#include "chain.hpp"
#include "jntarray.hpp"
#include "jacobian.hpp"
#include "tree.hpp"
#include "jntspaceinertiamatrix.hpp"
namespace KDL {
std::ostream& operator <<(std::ostream& os, const Joint& joint);
std::istream& operator >>(std::istream& is, Joint& joint);
std::ostream& operator <<(std::ostream& os, const Segment& segment);
std::istream& operator >>(std::istream& is, Segment& segment);
std::ostream& operator <<(std::ostream& os, const Chain& chain);
std::istream& operator >>(std::istream& is, Chain& chain);
std::ostream& operator <<(std::ostream& os, const Tree& tree);
std::istream& operator >>(std::istream& is, Tree& tree);
std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator it);
std::ostream& operator <<(std::ostream& os, const JntArray& array);
std::istream& operator >>(std::istream& is, JntArray& array);
std::ostream& operator <<(std::ostream& os, const Jacobian& jac);
std::istream& operator >>(std::istream& is, Jacobian& jac);
std::ostream& operator <<(std::ostream& os, const JntSpaceInertiaMatrix& jntspaceinertiamatrix);
std::istream& operator >>(std::istream& is, JntSpaceInertiaMatrix& jntspaceinertiamatrix);
//Builds a string containing the "branches" of a Tree using indentation or another
//user-supplied pattern, so that it is easier to visualize its structure. It is
//also possible to specify a "preamble", ie, a string to be included at the
//beginning of each new line.
std::string tree2str(const Tree& tree, const std::string& separator=" ", const std::string& preamble="");
std::string tree2str(const SegmentMap::const_iterator it, const std::string& separator=" ", const std::string& preamble="", unsigned int level=0);
/*
template<typename T>
std::ostream& operator<<(std::ostream& os, const std::vector<T>& vec) {
os << "[";
for (unsigned int i = 0; i < vec.size(); i++)
os << vec[i] << " ";
os << "]";
return os;
}
;
template<typename T>
std::istream& operator >>(std::istream& is, std::vector<T>& vec) {
return is;
}
;
*/
}
#endif

28
kdl_install/include/kdl/motion.hpp Normal file
View File

@@ -0,0 +1,28 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
/**
* @defgroup Motion Motion
* @brief All classes related to the non-instantaneous motion of rigid
* bodies and kinematic structures, e.g., path and trajecory definitions
* and their building blocks.
*
*/

136
kdl_install/include/kdl/path.hpp Normal file
View File

@@ -0,0 +1,136 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 path.h
path.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: path.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_MOTION_PATH_H
#define KDL_MOTION_PATH_H
#include "frames.hpp"
#include <vector>
#include "frames_io.hpp"
namespace KDL {
/**
* The specification of the path of a trajectory.
*/
class Path
{
public:
enum IdentifierType {
ID_LINE=1,
ID_CIRCLE=2,
ID_COMPOSITE=3,
ID_ROUNDED_COMPOSITE=4,
ID_POINT=5,
ID_CYCLIC_CLOSED=6
};
/**
* LengthToS() converts a physical length along the trajectory
* to the parameter s used in Pos, Vel and Acc. This is used because
* in cases with large rotations the parameter s does NOT correspond to
* the lineair length along the trajectory.
* User should be sure that the lineair distance travelled by this
* path object is NOT zero, when using this method !
* (e.g. the case of only rotational change)
* throws Error_MotionPlanning_Not_Applicable if used on composed
* path objects.
* @ingroup Motion
*/
virtual double LengthToS(double length) = 0;
/**
* Returns the total path length of the trajectory
* (has dimension LENGTH)
* This is not always a physical length , ie when dealing with rotations
* that are dominant.
*/
virtual double PathLength() = 0;
/**
* Returns the Frame at the current path length s
*/
virtual Frame Pos(double s) const = 0;
/**
* Returns the velocity twist at path length s theta and with
* derivative of s == sd
*/
virtual Twist Vel(double s,double sd) const = 0;
/**
* Returns the acceleration twist at path length s and with
* derivative of s == sd, and 2nd derivative of s == sdd
*/
virtual Twist Acc(double s,double sd,double sdd) const = 0;
/**
* Writes one of the derived objects to the stream
*/
virtual void Write(std::ostream& os) = 0;
/**
* Reads one of the derived objects from the stream and returns a pointer
* (factory method)
*/
static Path* Read(std::istream& is);
/**
* Virtual constructor, constructing by copying,
* Returns a deep copy of this Path Object
*/
virtual Path* Clone() = 0;
/**
* gets an identifier indicating the type of this Path object
*/
virtual IdentifierType getIdentifier() const=0;
virtual ~Path() {}
};
}
#endif

118
kdl_install/include/kdl/path_circle.hpp Normal file
View File

@@ -0,0 +1,118 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 path_circle.h
path_circle.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* ALTERNATIVE FOR trajectory_circle.h/cpp
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: path_circle.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_MOTION_PATHCIRCLE_H
#define KDL_MOTION_PATHCIRCLE_H
#include "path.hpp"
#include "rotational_interpolation.hpp"
namespace KDL {
/**
* A circular Path with 'open ends'. Path_Arc would
* have been a better name though.
* @ingroup Motion
*/
class Path_Circle : public Path
{
// Orientatie gedeelte
RotationalInterpolation* orient;
// Circular gedeelte
double radius;
Frame F_base_center;
// equivalent radius
double eqradius;
// verdeling baanlengte over pos/rot
double pathlength;
double scalelin;
double scalerot;
bool aggregate;
public:
/**
*
* CAN THROW Error_MotionPlanning_Circle_ToSmall
* CAN THROW Error_MotionPlanning_Circle_No_Plane
*/
Path_Circle(const Frame& F_base_start,const Vector& V_base_center,
const Vector& V_base_p,
const Rotation& R_base_end,
double alpha,
RotationalInterpolation* otraj,
double eqradius,
bool _aggregate=true);
double LengthToS(double length);
virtual double PathLength();
virtual Frame Pos(double s) const;
virtual Twist Vel(double s,double sd) const;
virtual Twist Acc(double s,double sd,double sdd) const;
virtual Path* Clone();
virtual void Write(std::ostream& os);
/**
* gets an identifier indicating the type of this Path object
*/
virtual IdentifierType getIdentifier() const {
return ID_CIRCLE;
}
virtual ~Path_Circle();
};
}
#endif

171
kdl_install/include/kdl/path_composite.hpp Normal file
View File

@@ -0,0 +1,171 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 path_composite.h
path_composite.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: path_composite.h,v 1.1.1.1.2.5 2003/07/24 13:49:16 rwaarsin Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_PATHCOMPOSITE_H
#define KDL_PATHCOMPOSITE_H
#include "frames.hpp"
#include "frames_io.hpp"
#include "path.hpp"
#include <vector>
namespace KDL {
/**
* A Path being the composition of other Path objects.
*
* For several of its methods, this class needs to lookup the segment corresponding to a value
* of the path variable s. To increase efficiency, this value is cached.
*
* \TODO Currently a linear search is used to look up the segment. A binary search is more efficient. Can STL be used for this ?
* \TODO Increase the efficiency for caching for the common case of a fine grained monotonously increasing path variable s.
*
* \TODO For all Path.., VelocityProfile.., Trajectory... check the bounds on the inputs with asserts.
*
* \TODO explain this routine in the wiki.
*
* @ingroup Motion
*/
class Path_Composite : public Path
{
typedef std::vector< std::pair<Path*,bool> > PathVector;
typedef std::vector<double> DoubleVector;
PathVector gv;
DoubleVector dv;
double pathlength;
// lookup mechanism :
mutable double cached_starts;
mutable double cached_ends;
mutable int cached_index;
double Lookup(double s) const;
public:
Path_Composite();
/**
* Adds a Path* to this composite
*/
void Add(Path* geom, bool aggregate=true);
virtual double LengthToS(double length);
/**
* Returns the total path length of the trajectory
* (has dimension LENGTH)
* This is not always a physical length , ie when dealing with rotations
* that are dominant.
*/
virtual double PathLength();
/**
* Returns the Frame at the current path length s
*/
virtual Frame Pos(double s) const;
/**
* Returns the velocity twist at path length s theta and with
* derivative of s == sd
*/
virtual Twist Vel(double s,double sd) const;
/**
* Returns the acceleration twist at path length s and with
* derivative of s == sd, and 2nd derivative of s == sdd
*/
virtual Twist Acc(double s,double sd,double sdd) const;
virtual Path* Clone();
/**
* Writes one of the derived objects to the stream
*/
virtual void Write(std::ostream& os);
/**
* returns the number of underlying segments.
*/
virtual int GetNrOfSegments();
/**
* returns a pointer to the underlying Path of the given segment number i.
* \param i segment number
* \return pointer to the underlying Path
* \warning The pointer is still owned by this class and is lifetime depends on the lifetime
* of this class.
*/
virtual Path* GetSegment(int i);
/**
* gets the length to the end of the given segment.
* \param i segment number
* \return length to the end of the segment, i.e. the value for s corresponding to the end of
* this segment.
*/
virtual double GetLengthToEndOfSegment(int i);
/**
* \param s [INPUT] path length variable for the composite.
* \param segment_number [OUTPUT] segments that corresponds to the path length variable s.
* \param inner_s [OUTPUT] path length to use within the segment.
*/
virtual void GetCurrentSegmentLocation(double s, int &segment_number, double& inner_s);
/**
* gets an identifier indicating the type of this Path object
*/
virtual IdentifierType getIdentifier() const {
return ID_COMPOSITE;
}
virtual ~Path_Composite();
};
}
#endif

90
kdl_install/include/kdl/path_cyclic_closed.hpp Normal file
View File

@@ -0,0 +1,90 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 path_cyclic_closed.h
path_cyclic_closed.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V2
*
* \par History
* - $log$
*
* \par Release
* $Id: path_cyclic_closed.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_MOTION_PATH_CYCLIC_CLOSED_H
#define KDL_MOTION_PATH_CYCLIC_CLOSED_H
#include "frames.hpp"
#include "frames_io.hpp"
#include "path.hpp"
#include <vector>
namespace KDL {
/**
* A Path representing a closed circular movement,
* which is traversed a number of times.
* @ingroup Motion
*/
class Path_Cyclic_Closed : public Path
{
int times;
Path* geom;
bool aggregate;
public:
Path_Cyclic_Closed(Path* _geom,int _times, bool _aggregate=true);
virtual double LengthToS(double length);
virtual double PathLength();
virtual Frame Pos(double s) const;
virtual Twist Vel(double s,double sd) const;
virtual Twist Acc(double s,double sd,double sdd) const;
virtual void Write(std::ostream& os);
static Path* Read(std::istream& is);
virtual Path* Clone();
/**
* gets an identifier indicating the type of this Path object
*/
virtual IdentifierType getIdentifier() const {
return ID_CYCLIC_CLOSED;
}
virtual ~Path_Cyclic_Closed();
};
}
#endif

138
kdl_install/include/kdl/path_line.hpp Normal file
View File

@@ -0,0 +1,138 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 path_line.h
path_line.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* ALTERNATIVE FOR trajectory_line.h/cpp
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: path_line.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_MOTION_PATH_LINE_H
#define KDL_MOTION_PATH_LINE_H
#include "path.hpp"
#include "rotational_interpolation.hpp"
namespace KDL {
/**
* A path representing a line from A to B.
* @ingroup Motion
*/
class Path_Line : public Path
{
// Orientatie gedeelte
RotationalInterpolation* orient;
// Lineair gedeelte
Vector V_base_start;
Vector V_base_end;
Vector V_start_end;
double eqradius; // equivalent radius
// verdeling baanlengte over pos/rot
double pathlength;
double scalelin;
double scalerot;
bool aggregate;
public:
/**
* Constructs a Line Path
* F_base_start and F_base_end give the begin and end frame wrt the base
* orient gives the method of rotation interpolation
* eqradius : equivalent radius :
* serves to compare rotations and translations.
* the "amount of motion"(pos,vel,acc) of the rotation is taken
* to be the amount motion of a point at distance eqradius from the
* rotation axis.
*
* Eqradius is introduced because it is unavoidable that you have to compare rotations and translations :
* e.g. : You can have motions that only contain rotation, and motions that only contain translations.
* The motion planning goes as follows :
* - translation is planned with the given parameters
* - rotation is planned planned with the parameters calculated with eqradius.
* - The longest of the previous two remains unchanged,
* the shortest in duration is scaled to take as long as the longest.
* This guarantees that the geometric path in 6D space remains independent of the motion profile parameters.
*
* RotationalInterpolation_SingleAxis() has the advantage that it is independent
* of the frame in which you express your path.
* Other implementations for RotationalInterpolations COULD be
* (not implemented) (yet) :
* 1) quaternion interpolation : but this is more difficult for the human to interpret
* 2) 3-axis interpolation : express the orientation of the frame in e.g.
* euler zyx angles alfa,beta, gamma and interpolate these parameters.
* But this is dependent of the frame you choose as a reference and
* their can occur representation singularities.
*/
Path_Line(const Frame& F_base_start,
const Frame& F_base_end,
RotationalInterpolation* orient,
double eqradius,
bool _aggregate=true);
Path_Line(const Frame& F_base_start,
const Twist& twist_in_base,
RotationalInterpolation* orient,
double eqradius,
bool _aggregate=true);
double LengthToS(double length);
virtual double PathLength();
virtual Frame Pos(double s) const;
virtual Twist Vel(double s,double sd) const ;
virtual Twist Acc(double s,double sd,double sdd) const;
virtual void Write(std::ostream& os);
virtual Path* Clone();
/**
* gets an identifier indicating the type of this Path object
*/
virtual IdentifierType getIdentifier() const {
return ID_LINE;
}
virtual ~Path_Line();
};
}
#endif

89
kdl_install/include/kdl/path_point.hpp Normal file
View File

@@ -0,0 +1,89 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 path_point.h
path_point.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* ALTERNATIVE FOR trajectory_stationary.h/cpp
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: path_point.h,v 1.1.2.3 2003/07/24 13:40:49 psoetens Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_MOTION_PATH_POINT_H
#define KDL_MOTION_PATH_POINT_H
#include "path.hpp"
#include "rotational_interpolation.hpp"
namespace KDL {
/**
* A Path consisting only of a point in space.
* @ingroup Motion
*/
class Path_Point : public Path
{
Frame F_base_start;
public:
/**
* Constructs a Point Path
*/
Path_Point(const Frame& F_base_start);
double LengthToS(double length);
virtual double PathLength();
virtual Frame Pos(double s) const;
virtual Twist Vel(double s,double sd) const ;
virtual Twist Acc(double s,double sd,double sdd) const;
virtual void Write(std::ostream& os);
virtual Path* Clone();
/**
* gets an identifier indicating the type of this Path object
*/
virtual IdentifierType getIdentifier() const {
return ID_POINT;
}
virtual ~Path_Point();
};
}
#endif

203
kdl_install/include/kdl/path_roundedcomposite.hpp Normal file
View File

@@ -0,0 +1,203 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 path_roundedcomposite.h
path_roundedcomposite.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: path_roundedcomposite.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_MOTION_ROUNDEDCOMPOSITE_H
#define KDL_MOTION_ROUNDEDCOMPOSITE_H
#include "path.hpp"
#include "path_composite.hpp"
#include "rotational_interpolation.hpp"
namespace KDL {
/**
* The specification of a path, composed of way-points with rounded corners.
*
* @ingroup Motion
*/
class Path_RoundedComposite : public Path
{
/** a Path_Composite is aggregated to hold the rounded trajectory
* with circles and lines
*/
Path_Composite* comp;
double radius;
double eqradius;
RotationalInterpolation* orient;
// cached from underlying path objects for generating the rounding :
Frame F_base_start;
Frame F_base_via;
//Frame F_base_end;
int nrofpoints;
bool aggregate;
Path_RoundedComposite(Path_Composite* comp,double radius,double eqradius,RotationalInterpolation* orient, bool aggregate, int nrofpoints);
public:
/**
* @param radius : radius of the rounding circles
* @param eqradius : equivalent radius to compare rotations/velocities
* @param orient : method of rotational_interpolation interpolation
* @param aggregate : if true, this object will own the _orient pointer, i.e. it will delete the _orient pointer
* when the destructor of this object is called.
*/
Path_RoundedComposite(double radius,double eqradius,RotationalInterpolation* orient, bool aggregate=true);
/**
* Adds a point to this rounded composite, between two adjacent points
* a Path_Line will be created, between two lines there will be
* rounding with the given radius with a Path_Circle
*
* The Error_MotionPlanning_Not_Feasible has a type (obtained by GetType) of:
* - 3101 if the eq. radius <= 0
* - 3102 if the first segment in a rounding has zero length.
* - 3103 if the second segment in a rounding has zero length.
* - 3104 if the angle between the first and the second segment is close to PI.
* (meaning that the segments are on top of each other)
* - 3105 if the distance needed for the rounding is larger then the first segment.
* - 3106 if the distance needed for the rounding is larger then the second segment.
*
* @param F_base_point the pose of a new via point.
* @warning Can throw Error_MotionPlanning_Not_Feasible object
* @TODO handle the case of error type 3105 and 3106 by skipping segments, such that the class could be applied
* with points that are very close to each other.
*/
void Add(const Frame& F_base_point);
/**
* to be called after the last line is added to finish up
* the work
*/
void Finish();
virtual double LengthToS(double length);
/**
* Returns the total path length of the trajectory
* (has dimension LENGTH)
* This is not always a physical length , ie when dealing with rotations
* that are dominant.
*/
virtual double PathLength();
/**
* Returns the Frame at the current path length s
*/
virtual Frame Pos(double s) const;
/**
* Returns the velocity twist at path length s theta and with
* derivative of s == sd
*/
virtual Twist Vel(double s,double sd) const;
/**
* Returns the acceleration twist at path length s and with
* derivative of s == sd, and 2nd derivative of s == sdd
*/
virtual Twist Acc(double s,double sd,double sdd) const;
/**
* virtual constructor, constructing by copying.
* In this case it returns the Clone() of the aggregated Path_Composite
* because this is all one ever will need.
*/
virtual Path* Clone();
/**
* Writes one of the derived objects to the stream
*/
virtual void Write(std::ostream& os);
/**
* returns the number of underlying segments.
*/
virtual int GetNrOfSegments();
/**
* returns a pointer to the underlying Path of the given segment number i.
* \param i segment number
* \return pointer to the underlying Path
* \warning The pointer is still owned by this class and is lifetime depends on the lifetime
* of this class.
*/
virtual Path* GetSegment(int i);
/**
* gets the length to the end of the given segment.
* \param i segment number
* \return length to the end of the segment, i.e. the value for s corresponding to the end of
* this segment.
*/
virtual double GetLengthToEndOfSegment(int i);
/**
* \param s [INPUT] path length variable for the composite.
* \param segment_number [OUTPUT] segments that corresponds to the path length variable s.
* \param inner_s [OUTPUT] path length to use within the segment.
*/
virtual void GetCurrentSegmentLocation(double s, int &segment_number, double& inner_s);
/**
* gets an identifier indicating the type of this Path object
*/
virtual IdentifierType getIdentifier() const {
return ID_ROUNDED_COMPOSITE;
}
virtual ~Path_RoundedComposite();
};
}
#endif

140
kdl_install/include/kdl/rigidbodyinertia.hpp Normal file
View File

@@ -0,0 +1,140 @@
// Copyright (C) 2009 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_RIGIDBODYINERTIA_HPP
#define KDL_RIGIDBODYINERTIA_HPP
#include "frames.hpp"
#include "rotationalinertia.hpp"
namespace KDL {
/**
* \brief 6D Inertia of a rigid body
*
* The inertia is defined in a certain reference point and a certain reference base.
* The reference point does not have to coincide with the origin of the reference frame.
*/
class RigidBodyInertia{
public:
/**
* This constructor creates a cartesian space inertia matrix,
* the arguments are the mass, the vector from the reference point to cog and the rotational inertia in the cog.
*/
explicit RigidBodyInertia(double m=0, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero());
/**
* Creates an inertia with zero mass, and zero RotationalInertia
*/
static inline RigidBodyInertia Zero(){
return RigidBodyInertia(0.0,Vector::Zero(),RotationalInertia::Zero());
};
~RigidBodyInertia(){};
friend RigidBodyInertia operator*(double a,const RigidBodyInertia& I);
friend RigidBodyInertia operator+(const RigidBodyInertia& Ia,const RigidBodyInertia& Ib);
friend Wrench operator*(const RigidBodyInertia& I,const Twist& t);
friend RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I);
friend RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I);
/**
* Reference point change with v the vector from the old to
* the new point expressed in the current reference frame
*/
RigidBodyInertia RefPoint(const Vector& p);
/**
* Get the mass of the rigid body
*/
double getMass() const{
return m;
};
/**
* Get the spatial momentum of the rigid body
*/
const Vector& getSpatialMomentum() const
{
return h;
}
/**
* Get the center of gravity of the rigid body
*/
Vector getCOG() const{
if(m==0) return Vector::Zero();
else return h/m;
};
/**
* Get the rotational inertia expressed in the reference frame (not the cog)
*/
RotationalInertia getRotationalInertia() const{
return I;
};
private:
RigidBodyInertia(double m,const Vector& h,const RotationalInertia& I,bool mhi);
double m;
Vector h;
RotationalInertia I;
friend class ArticulatedBodyInertia;
};
/**
* Scalar product: I_new = double * I_old
*/
RigidBodyInertia operator*(double a,const RigidBodyInertia& I);
/**
* addition I: I_new = I_old1 + I_old2, make sure that I_old1
* and I_old2 are expressed in the same reference frame/point,
* otherwise the result is worth nothing
*/
RigidBodyInertia operator+(const RigidBodyInertia& Ia,const RigidBodyInertia& Ib);
/**
* calculate spatial momentum: h = I*v
* make sure that the twist v and the inertia are expressed in the same reference frame/point
*/
Wrench operator*(const RigidBodyInertia& I,const Twist& t);
/**
* Coordinate system transform Ia = T_a_b*Ib with T_a_b the frame from a to b.
*/
RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I);
/**
* Reference frame orientation change Ia = R_a_b*Ib with R_a_b
* the rotation of b expressed in a
*/
RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I);
}//namespace
#endif

118
kdl_install/include/kdl/rotational_interpolation.hpp Normal file
View File

@@ -0,0 +1,118 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 rotational_interpolation.h
rotational_interpolation.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: rotational_interpolation.h,v 1.1.1.1.2.2 2003/02/24 13:13:06 psoetens Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_ROTATIONALINTERPOLATION_H
#define KDL_ROTATIONALINTERPOLATION_H
#include "frames.hpp"
#include "frames_io.hpp"
namespace KDL {
/**
* RotationalInterpolation specifies the rotational part of a geometric trajectory
* - The different derived objects specify different methods for interpolating
* rotations.
* - SetStartEnd should be called before
* using the other methods
* - The start and end position do NOT belong to the persistent state ! The owner of this
* object is responsible for setting these each time
* @ingroup Motion
*/
class RotationalInterpolation
{
public:
/**
* Set the start and end rotational_interpolation
*/
virtual void SetStartEnd(Rotation start,Rotation end) = 0;
/**
* - Returns the angle value to move from start to end.
* This should have units radians,
* - With Single Axis interp corresponds to the angle rotation
* - With Three Axis interp corresponds to the slowest of the three
* rotations.
*/
virtual double Angle() = 0;
/**
* Returns the rotation matrix at angle theta
*/
virtual Rotation Pos(double theta) const = 0;
/**
* Returns the rotational velocity at angle theta and with
* derivative of theta == thetad
*/
virtual Vector Vel(double theta,double thetad) const = 0;
/**
* Returns the rotational acceleration at angle theta and with
* derivative of theta == thetad, and 2nd derivative of theta == thdd
*/
virtual Vector Acc(double theta,double thetad,double thetadd) const = 0;
/**
* Writes one of the derived objects to the stream
*/
virtual void Write(std::ostream& os) const = 0;
/**
* Reads one of the derived objects from the stream and returns a pointer
* (factory method)
*/
static RotationalInterpolation* Read(std::istream& is);
/**
* virtual constructor, construction by copying ..
*/
virtual RotationalInterpolation* Clone() const = 0;
virtual ~RotationalInterpolation() {}
};
}
#endif

84
kdl_install/include/kdl/rotational_interpolation_sa.hpp Normal file
View File

@@ -0,0 +1,84 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 rotational_interpolation_sa.h
rotational_interpolation_sa.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: rotational_interpolation_singleaxis.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_ROTATIONALINTERPOLATION_SINGLEAXIS_H
#define KDL_ROTATIONALINTERPOLATION_SINGLEAXIS_H
#include "frames.hpp"
#include "frames_io.hpp"
#include "rotational_interpolation.hpp"
namespace KDL {
/**
* An interpolation algorithm which rotates a frame over the existing
* single rotation axis
* formed by start and end rotation. If more than one rotational axis
* exist, an arbitrary one will be chosen, therefore it is not recommended
* to try to interpolate a 180 degrees rotation.
* @ingroup Motion
*/
class RotationalInterpolation_SingleAxis: public RotationalInterpolation
{
Rotation R_base_start;
Rotation R_base_end;
Vector rot_start_end;
double angle;
public:
RotationalInterpolation_SingleAxis();
virtual void SetStartEnd(Rotation start,Rotation end);
virtual double Angle();
virtual Rotation Pos(double th) const;
virtual Vector Vel(double th,double thd) const;
virtual Vector Acc(double th,double thd,double thdd) const;
virtual void Write(std::ostream& os) const;
virtual RotationalInterpolation* Clone() const;
virtual ~RotationalInterpolation_SingleAxis();
};
}
#endif

74
kdl_install/include/kdl/rotationalinertia.hpp Normal file
View File

@@ -0,0 +1,74 @@
// Copyright (C) 2009 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_ROTATIONALINERTIA_HPP
#define KDL_ROTATIONALINERTIA_HPP
#include "frames.hpp"
//------- class for only the Rotational Inertia --------
namespace KDL
{
//Forward declaration
class RigidBodyInertia;
class RotationalInertia{
public:
explicit RotationalInertia(double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0);
static inline RotationalInertia Zero(){
return RotationalInertia(0,0,0,0,0,0);
};
friend RotationalInertia operator*(double a, const RotationalInertia& I);
friend RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib);
/**
* This function calculates the angular momentum resulting from a rotational velocity omega
*/
KDL::Vector operator*(const KDL::Vector& omega) const;
~RotationalInertia();
friend class RigidBodyInertia;
///Scalar product
friend RigidBodyInertia operator*(double a,const RigidBodyInertia& I);
///addition
friend RigidBodyInertia operator+(const RigidBodyInertia& Ia,const RigidBodyInertia& Ib);
///calculate spatial momentum
friend Wrench operator*(const RigidBodyInertia& I,const Twist& t);
///coordinate system transform Ia = T_a_b*Ib with T_a_b the frame from a to b
friend RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I);
///base frame orientation change Ia = R_a_b*Ib with R_a_b the rotation for frame from a to b
friend RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I);
double data[9];
};
RotationalInertia operator*(double a, const RotationalInertia& I);
RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib);
}
#endif

177
kdl_install/include/kdl/segment.hpp Normal file
View File

@@ -0,0 +1,177 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at intermodalics dot eu>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_SEGMENT_HPP
#define KDL_SEGMENT_HPP
#include "frames.hpp"
#include "rigidbodyinertia.hpp"
#include "joint.hpp"
#include <vector>
namespace KDL {
/**
* \brief This class encapsulates a simple segment, that is a "rigid
* body" (i.e., a frame and a rigid body inertia) with a joint and with
* "handles", root and tip to connect to other segments.
*
* A simple segment is described by the following properties :
* - Joint
* - Rigid Body Inertia: of the rigid body part of the Segment
* - Offset from the end of the joint to the tip of the segment:
* the joint is located at the root of the segment.
*
* @ingroup KinematicFamily
*/
class Segment {
friend class Chain;
private:
std::string name;
Joint joint;
RigidBodyInertia I;
Frame f_tip;
public:
/**
* Constructor of the segment
*
* @param name name of the segment
* @param joint joint of the segment, default:
* Joint(Joint::Fixed)
* @param f_tip frame from the end of the joint to the tip of
* the segment, default: Frame::Identity()
* @param M rigid body inertia of the segment, default: Inertia::Zero()
*/
explicit Segment(const std::string& name, const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
/**
* Constructor of the segment
*
* @param joint joint of the segment, default:
* Joint(Joint::Fixed)
* @param f_tip frame from the end of the joint to the tip of
* the segment, default: Frame::Identity()
* @param M rigid body inertia of the segment, default: Inertia::Zero()
*/
explicit Segment(const Joint& joint=Joint(Joint::Fixed), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
Segment(const Segment& in);
Segment& operator=(const Segment& arg);
virtual ~Segment();
/**
* Request the pose of the segment, given the joint position q.
*
* @param q 1D position of the joint
*
* @return pose from the root to the tip of the segment
*/
Frame pose(const double& q)const;
/**
* Request the 6D-velocity of the tip of the segment, given
* the joint position q and the joint velocity qdot.
*
* @param q 1D position of the joint
* @param qdot 1D velocity of the joint
*
* @return 6D-velocity of the tip of the segment, expressed
*in the base-frame of the segment(root) and with the tip of
*the segment as reference point.
*/
Twist twist(const double& q,const double& qdot)const;
/**
* Request the name of the segment
*
*
* @return const reference to the name of the segment
*/
const std::string& getName()const
{
return name;
}
/**
* Request the joint of the segment
*
*
* @return const reference to the joint of the segment
*/
const Joint& getJoint()const
{
return joint;
}
/**
* Request the inertia of the segment
*
*
* @return const reference to the inertia of the segment
*/
const RigidBodyInertia& getInertia()const
{
return I;
}
/**
* Request the inertia of the segment
*
*
* @return const reference to the inertia of the segment
*/
void setInertia(const RigidBodyInertia& Iin)
{
this->I=Iin;
}
/**
* Request the pose from the joint end to the tip of the
*segment.
*
* @return the original parent end - segment end pose.
*/
Frame getFrameToTip()const
{
return joint.pose(0)*f_tip;
}
/**
* Set the pose from the joint end to the tip of the
* segment.
*
* @param f_tip_new pose from the joint end to the tip of the segment
*/
void setFrameToTip(const Frame& f_tip_new);
/**
* Request the pose from the end of the joint to the tip of the segment
* at joint position 0.
*
* @return const reference to the pose from the end of the joint to the tip of the segment
* at joint position 0
*/
const Frame& getFrameToTipZero() const
{
return f_tip;
}
};
}//end of namespace KDL
#endif

154
kdl_install/include/kdl/solveri.hpp Normal file
View File

@@ -0,0 +1,154 @@
// Copyright (C) 2013 Stephen Roderick <kiwi dot net at mac dot com>
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef __SOLVERI_HPP
#define __SOLVERI_HPP
namespace KDL {
/**
* Solver interface supporting storage and description of the latest error.
*
* Error codes: Zero (0) indicates no error, positive error codes indicate more
* of a warning (e.g. a degraded solution, but motion can continue), and
* negative error codes indicate failure (e.g. a singularity, and motion
* can not continue).
*
* Error codes between -99 and +99 (inclusive) are reserved for system-wide
* error codes. Derived classes should use values > +100, and < -100.
*
* Example use
*
* \code
* class MySolver : public SolverI
* {
* public:
* static const int E_CHILDFAILD = xxx;
*
* MySolver(SomeOtherSolver& other);
* virtual ~MySolver();
* int CartToJnt(...);
* virtual const char* strError(const int error) const;
* protected:
* SomeOtherSolver& child;
* };
*
* ...
*
* int MySolver::CartToJnt(...)
* {
* error = child->SomeCall();
* if (E_NOERROR != error) {
* error = E_CHILDFAILED;
* } else {
* ...
* }
* return error;
* }
*
* const char* MySolver::strError(const int error) const
* {
* if (E_CHILDFAILED == error) return "Child solver failed";
* else return SolverI::strError(error);
* }
*
* void someFunc()
* {
* SomeOtherSolver child = new SomeOtherSolver(...);
* MySolver parent = new MySolver(child);
* ...
* int rc = parent->CartToJnt(...);
* if (E_NOERROR != rc) {
* if (MySolver::E_CHILDFAILED == rc) {
* rc = child->getError();
* // cope with child failure 'rc'
* }
* }
* ...
* }
* \endcode
*/
class SolverI
{
public:
enum {
/// Converged but degraded solution (e.g. WDLS with psuedo-inverse singular)
E_DEGRADED = +1,
//! No error
E_NOERROR = 0,
//! Failed to converge
E_NO_CONVERGE = -1,
//! Undefined value (e.g. computed a NAN, or tan(90 degrees) )
E_UNDEFINED = -2,
//! Chain size changed
E_NOT_UP_TO_DATE = -3,
//! Input size does not match internal state
E_SIZE_MISMATCH = -4,
//! Maximum number of iterations exceeded
E_MAX_ITERATIONS_EXCEEDED = -5,
//! Requested index out of range
E_OUT_OF_RANGE = -6,
//! Not yet implemented
E_NOT_IMPLEMENTED = -7,
//! Internal svd calculation failed
E_SVD_FAILED = -8
};
/// Initialize latest error to E_NOERROR
SolverI() :
error(E_NOERROR)
{}
virtual ~SolverI()
{}
/// Return the latest error
virtual int getError() const { return error; }
/** Return a description of the latest error
\return if \a error is known then a description of \a error, otherwise
"UNKNOWN ERROR"
*/
virtual const char* strError(const int error) const
{
if (E_NOERROR == error) return "No error";
else if (E_NO_CONVERGE == error) return "Failed to converge";
else if (E_UNDEFINED == error) return "Undefined value";
else if (E_DEGRADED == error) return "Converged but degraded solution";
else if (E_NOT_UP_TO_DATE == error) return "Internal data structures not up to date with Chain";
else if (E_SIZE_MISMATCH == error) return "The size of the input does not match the internal state";
else if (E_MAX_ITERATIONS_EXCEEDED == error) return "The maximum number of iterations is exceeded";
else if (E_OUT_OF_RANGE == error) return "The requested index is out of range";
else if (E_NOT_IMPLEMENTED == error) return "The requested function is not yet implemented";
else if (E_SVD_FAILED == error) return "SVD failed";
else return "UNKNOWN ERROR";
}
/**
* Update the internal data structures. This is required if the number
* of segments or number of joints of a chain/tree have changed.
* This provides a single point of contact for solver memory allocations.
*/
virtual void updateInternalDataStructures() = 0;
protected:
/// Latest error, initialized to E_NOERROR in constructor
int error;
};
} // namespaces
#endif

120
kdl_install/include/kdl/stiffness.hpp Normal file
View File

@@ -0,0 +1,120 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_STIFFNESS_H
#define KDL_STIFFNESS_H
#include "frames.hpp"
namespace KDL {
/**
* Preliminary class to implement Stiffness, only diagonal stiffness is implemented
* no transformations provided...
*
* Implements a diagonal stiffness matrix.
* first 3 elements are stiffness for translations
* last 3 elements are stiffness for rotations.
*/
class Stiffness {
double data[6];
public:
Stiffness() {
data[0]=0;
data[1]=0;
data[2]=0;
data[3]=0;
data[4]=0;
data[5]=0;
}
Stiffness(double* d) {
data[0]=d[0];
data[1]=d[1];
data[2]=d[2];
data[3]=d[3];
data[4]=d[4];
data[5]=d[5];
}
Stiffness(double x,double y,double z,double rx,double ry,double rz) {
data[0]=x;
data[1]=y;
data[2]=z;
data[3]=rx;
data[4]=ry;
data[5]=rz;
}
double& operator[](int i) {
return data[i];
}
double operator[](int i) const {
return data[i];
}
Twist Inverse(const Wrench& w) const{
Twist t;
t[0]=w[0]/data[0];
t[1]=w[1]/data[1];
t[2]=w[2]/data[2];
t[3]=w[3]/data[3];
t[4]=w[4]/data[4];
t[5]=w[5]/data[5];
return t;
}
};
inline Wrench operator * (const Stiffness& s, const Twist& t) {
Wrench w;
w[0]=s[0]*t[0];
w[1]=s[1]*t[1];
w[2]=s[2]*t[2];
w[3]=s[3]*t[3];
w[4]=s[4]*t[4];
w[5]=s[5]*t[5];
return w;
}
inline Stiffness operator+(const Stiffness& s1, const Stiffness& s2) {
Stiffness s;
s[0]=s1[0]+s2[0];
s[1]=s1[1]+s2[1];
s[2]=s1[2]+s2[2];
s[3]=s1[3]+s2[3];
s[4]=s1[4]+s2[4];
s[5]=s1[5]+s2[5];
return s;
}
inline void posrandom(Stiffness& F) {
posrandom(F[0]);
posrandom(F[1]);
posrandom(F[2]);
posrandom(F[3]);
posrandom(F[4]);
posrandom(F[5]);
}
inline void random(Stiffness& F) {
posrandom(F);
}
}
#endif

105
kdl_install/include/kdl/trajectory.hpp Normal file
View File

@@ -0,0 +1,105 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 trajectory.h
trajectory.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: trajectory.h,v 1.1.1.1.2.5 2003/07/23 16:44:25 psoetens Exp $
* $Name: $
* \todo
* Peter's remark : should separate I/O from other routines in the
* motion/chain directories
* The problem is that the I/O uses virtual inheritance to write
* the trajectories/geometries/velocityprofiles/...
* Have no good solution for this, perhaps
* * #ifdef's
* * declaring dummy ostream/istream and change implementation file .cpp
* * declaring some sort of VISITOR object (containing an ostream) ,
* the classes contain code to pass this object around along its children
* a subroutine can then be called with overloading.
* PROBLEM : if you declare a friend you have to fully declare it ==> exposing I/O with ostream/istream decl
* CONSEQUENCE : everything has to be declared public.
****************************************************************************/
#ifndef TRAJECTORY_H
#define TRAJECTORY_H
#include "frames.hpp"
#include "frames_io.hpp"
#include "path.hpp"
#include "velocityprofile.hpp"
namespace KDL {
/**
* An abstract class that implements
* a trajectory contains a cartesian space trajectory and an underlying
* velocity profile.
* @ingroup Motion
*/
class Trajectory
{
public:
virtual double Duration() const = 0;
// The duration of the trajectory
virtual Frame Pos(double time) const = 0;
// Position of the trajectory at <time>.
virtual Twist Vel(double time) const = 0;
// The velocity of the trajectory at <time>.
virtual Twist Acc(double time) const = 0;
// The acceleration of the trajectory at <time>.
virtual Trajectory* Clone() const = 0;
virtual void Write(std::ostream& os) const = 0;
static Trajectory* Read(std::istream& is);
virtual ~Trajectory() {}
// note : you cannot declare this destructor abstract
// it is always called by the descendant's destructor !
};
}
#endif

63
kdl_install/include/kdl/trajectory_composite.hpp Normal file
View File

@@ -0,0 +1,63 @@
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* LRL V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: trajectory_composite.h 22 2004-09-21 08:58:54Z eaertbellocal $
* $Name: $
****************************************************************************/
#ifndef TRAJECTORY_COMPOSITE_H
#define TRAJECTORY_COMPOSITE_H
#include "trajectory.hpp"
#include "path_composite.hpp"
#include <vector>
namespace KDL {
/**
* Trajectory_Composite implements a trajectory that is composed
* of underlying trajectoria. Call Add to add a trajectory
* @ingroup Motion
*/
class Trajectory_Composite: public Trajectory
{
typedef std::vector<Trajectory*> VectorTraj;
typedef std::vector<double> VectorDouble;
VectorTraj vt; // contains the element Trajectories
VectorDouble vd; // contains end time for each Trajectory
double duration; // total duration of the composed
// Trajectory
public:
Trajectory_Composite();
// Constructs an empty composite
virtual double Duration() const;
virtual Frame Pos(double time) const;
virtual Twist Vel(double time) const;
virtual Twist Acc(double time) const;
virtual void Add(Trajectory* elem);
// Adds trajectory <elem> to the end of the sequence.
virtual void Destroy();
virtual void Write(std::ostream& os) const;
virtual Trajectory* Clone() const;
virtual ~Trajectory_Composite();
};
}
#endif

112
kdl_install/include/kdl/trajectory_segment.hpp Normal file
View File

@@ -0,0 +1,112 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 trajectory_segment.h
trajectory_segment.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: trajectory_segment.h,v 1.1.1.1.2.5 2003/07/23 16:44:26 psoetens Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_MOTION_TRAJECTORY_SEGMENT_H
#define KDL_MOTION_TRAJECTORY_SEGMENT_H
#include "frames.hpp"
#include "frames_io.hpp"
#include "trajectory.hpp"
#include "path.hpp"
#include "velocityprofile.hpp"
namespace KDL {
/**
* Trajectory_Segment combines a VelocityProfile and a Path into a
* trajectory
* @ingroup Motion
*/
class Trajectory_Segment : public Trajectory
{
VelocityProfile* motprof;
Path* geom;
bool aggregate;
public:
/**
* This constructor assumes that \a geom and <_motprof> are initialised correctly.
*/
Trajectory_Segment(Path* _geom, VelocityProfile* _motprof, bool _aggregate=true);
/**
* This constructor assumes that \a geom is initialised and <_motprof> needs to be
* set according to \a duration.
*/
Trajectory_Segment(Path* _geom, VelocityProfile* _motprof, double duration, bool _aggregate=true);
virtual double Duration() const;
// The duration of the trajectory
virtual Frame Pos(double time) const;
// Position of the trajectory at <time>.
virtual Twist Vel(double time) const;
// The velocity of the trajectory at <time>.
virtual Twist Acc(double time) const;
// The acceleration of the trajectory at <time>.
virtual Trajectory* Clone() const
{
if ( aggregate )
return new Trajectory_Segment( geom->Clone(), motprof->Clone(), true );
return new Trajectory_Segment( geom, motprof, false );
}
virtual void Write(std::ostream& os) const;
virtual Path* GetPath();
virtual VelocityProfile* GetProfile();
virtual ~Trajectory_Segment();
};
}
#endif

58
kdl_install/include/kdl/trajectory_stationary.hpp Normal file
View File

@@ -0,0 +1,58 @@
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* LRL V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: trajectory_stationary.h 22 2004-09-21 08:58:54Z eaertbellocal $
* $Name: $
****************************************************************************/
#ifndef TRAJECTORY_STATIONARY_H
#define TRAJECTORY_STATIONARY_H
#include "trajectory.hpp"
namespace KDL {
/**
* Implements a "trajectory" of a stationary position
* for an amount of time.
* @ingroup Motion
*/
class Trajectory_Stationary : public Trajectory
{
double duration;
Frame pos;
public:
Trajectory_Stationary(double _duration,const Frame& _pos):
duration(_duration),pos(_pos) {}
virtual double Duration() const {
return duration;
}
virtual Frame Pos(double time) const {
return pos;
}
virtual Twist Vel(double time) const {
return Twist::Zero();
}
virtual Twist Acc(double time) const {
return Twist::Zero();
}
virtual void Write(std::ostream& os) const;
virtual Trajectory* Clone() const {
return new Trajectory_Stationary(duration,pos);
}
virtual ~Trajectory_Stationary() {}
};
}
#endif

227
kdl_install/include/kdl/tree.hpp Normal file
View File

@@ -0,0 +1,227 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_TREE_HPP
#define KDL_TREE_HPP
#include "config.h"
#include "segment.hpp"
#include "chain.hpp"
#include <string>
#include <map>
#ifdef KDL_USE_NEW_TREE_INTERFACE
#include <boost/shared_ptr.hpp>
#endif //#ifdef KDL_USE_NEW_TREE_INTERFACE
namespace KDL
{
class TreeElement;
#ifdef KDL_USE_NEW_TREE_INTERFACE
//We use smart pointers for managing tree nodes for now because
//c++11 and unique_ptr support is not ubiquitous
typedef boost::shared_ptr<TreeElement> TreeElementPtr;
typedef boost::shared_ptr<const TreeElement> TreeElementConstPtr;
typedef std::map<std::string, TreeElementPtr> SegmentMap;
typedef TreeElementPtr TreeElementType;
#define GetTreeElementChildren(tree_element) (tree_element)->children
#define GetTreeElementParent(tree_element) (tree_element)->parent
#define GetTreeElementQNr(tree_element) (tree_element)->q_nr
#define GetTreeElementSegment(tree_element) (tree_element)->segment
#else //#ifdef KDL_USE_NEW_TREE_INTERFACE
//Forward declaration
typedef std::map<std::string,TreeElement> SegmentMap;
typedef TreeElement TreeElementType;
#define GetTreeElementChildren(tree_element) (tree_element).children
#define GetTreeElementParent(tree_element) (tree_element).parent
#define GetTreeElementQNr(tree_element) (tree_element).q_nr
#define GetTreeElementSegment(tree_element) (tree_element).segment
#endif //#ifdef KDL_USE_NEW_TREE_INTERFACE
class TreeElement
{
public:
TreeElement(const Segment& segment_in,const SegmentMap::const_iterator& parent_in,unsigned int q_nr_in):
segment(segment_in),
q_nr(q_nr_in),
parent(parent_in)
{}
static TreeElementType Root(const std::string& root_name)
{
#ifdef KDL_USE_NEW_TREE_INTERFACE
return TreeElementType(new TreeElement(root_name));
#else //#define KDL_USE_NEW_TREE_INTERFACE
return TreeElementType(root_name);
#endif
}
Segment segment;
unsigned int q_nr;
SegmentMap::const_iterator parent;
std::vector<SegmentMap::const_iterator > children;
private:
TreeElement(const std::string& name):segment(name), q_nr(0) {}
};
/**
* \brief This class encapsulates a <strong>tree</strong>
* kinematic interconnection structure. It is built out of segments.
*
* @ingroup KinematicFamily
*/
class Tree
{
private:
SegmentMap segments;
unsigned int nrOfJoints;
unsigned int nrOfSegments;
std::string root_name;
bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& hook_name);
public:
/**
* The constructor of a tree, a new tree is always empty
*/
explicit Tree(const std::string& root_name="root");
Tree(const Tree& in);
Tree& operator= (const Tree& arg);
/**
* Adds a new segment to the end of the segment with
* hook_name as segment_name
*
* @param segment new segment to add
* @param hook_name name of the segment to connect this
* segment with.
*
* @return false if hook_name could not be found.
*/
bool addSegment(const Segment& segment, const std::string& hook_name);
/**
* Adds a complete chain to the end of the segment with
* hook_name as segment_name.
*
* @param hook_name name of the segment to connect the chain with.
*
* @return false if hook_name could not be found.
*/
bool addChain(const Chain& chain, const std::string& hook_name);
/**
* Adds a complete tree to the end of the segment with
* hookname as segment_name.
*
* @param tree Tree to add
* @param hook_name name of the segment to connect the tree with
*
* @return false if hook_name could not be found
*/
bool addTree(const Tree& tree, const std::string& hook_name);
/**
* Request the total number of joints in the tree.\n
* <strong> Important:</strong> It is not the same as the
* total number of segments since a segment does not need to have
* a joint.
*
* @return total nr of joints
*/
unsigned int getNrOfJoints()const
{
return nrOfJoints;
};
/**
* Request the total number of segments in the tree.
* @return total number of segments
*/
unsigned int getNrOfSegments()const {return nrOfSegments;};
/**
* Request the segment of the tree with name segment_name.
*
* @param segment_name the name of the requested segment
*
* @return constant iterator pointing to the requested segment
*/
SegmentMap::const_iterator getSegment(const std::string& segment_name)const
{
return segments.find(segment_name);
};
/**
* Request the root segment of the tree
*
* @return constant iterator pointing to the root segment
*/
SegmentMap::const_iterator getRootSegment()const
{
return segments.find(root_name);
};
/**
* Request the chain of the tree between chain_root and chain_tip. The chain_root
* and chain_tip can be in different branches of the tree, the chain_root can be
* an ancestor of chain_tip, and chain_tip can be an ancestor of chain_root.
*
* @param chain_root the name of the root segment of the chain
* @param chain_tip the name of the tip segment of the chain
* @param chain the resulting chain
*
* @return success or failure
*/
bool getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const;
/**
* Extract a tree having segment_name as root. Only child segments of
* segment_name are added to the new tree.
*
* @param segment_name the name of the segment to be used as root
* of the new tree
* @param tree the resulting sub-tree
*
* @return success or failure
*/
bool getSubTree(const std::string& segment_name, Tree& tree)const;
const SegmentMap& getSegments()const
{
return segments;
}
virtual ~Tree(){};
};
}
#endif

110
kdl_install/include/kdl/treefksolver.hpp Normal file
View File

@@ -0,0 +1,110 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Copyright (C) 2008 Julia Jesse
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_TREE_FKSOLVER_HPP
#define KDL_TREE_FKSOLVER_HPP
#include <string>
#include "tree.hpp"
//#include "framevel.hpp"
//#include "frameacc.hpp"
#include "jntarray.hpp"
//#include "jntarrayvel.hpp"
//#include "jntarrayacc.hpp"
namespace KDL {
/**
* \brief This <strong>abstract</strong> class encapsulates a
* solver for the forward position kinematics for a KDL::Tree.
*
* @ingroup KinematicFamily
*/
//Forward definition
class TreeFkSolverPos {
public:
/**
* Calculate forward position kinematics for a KDL::Tree,
* from joint coordinates to cartesian pose.
*
* @param q_in input joint coordinates
* @param p_out reference to output cartesian pose
*
* @return if < 0 something went wrong
*/
virtual int JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName)=0;
virtual ~TreeFkSolverPos(){};
};
/**
* \brief This <strong>abstract</strong> class encapsulates a solver
* for the forward velocity kinematics for a KDL::Tree.
*
* @ingroup KinematicFamily
*/
// class TreeFkSolverVel {
// public:
/**
* Calculate forward position and velocity kinematics, from
* joint coordinates to cartesian coordinates.
*
* @param q_in input joint coordinates (position and velocity)
* @param out output cartesian coordinates (position and velocity)
*
* @return if < 0 something went wrong
*/
// virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0;
// virtual ~TreeFkSolverVel(){};
// };
/**
* \brief This <strong>abstract</strong> class encapsulates a solver
* for the forward acceleration kinematics for a KDL::Tree.
*
* @ingroup KinematicFamily
*/
// class TreeFkSolverAcc {
// public:
/**
* Calculate forward position, velocity and acceleration
* kinematics, from joint coordinates to cartesian coordinates
*
* @param q_in input joint coordinates (position, velocity and
* acceleration
@param out output cartesian coordinates (position, velocity
* and acceleration
*
* @return if < 0 something went wrong
*/
// virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0;
// virtual ~TreeFkSolverAcc()=0;
// };
}//end of namespace KDL
#endif

53
kdl_install/include/kdl/treefksolverpos_recursive.hpp Normal file
View File

@@ -0,0 +1,53 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Copyright (C) 2008 Julia Jesse
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDLTREEFKSOLVERPOS_RECURSIVE_HPP
#define KDLTREEFKSOLVERPOS_RECURSIVE_HPP
#include "treefksolver.hpp"
namespace KDL {
/**
* Implementation of a recursive forward position kinematics
* algorithm to calculate the position transformation from joint
* space to Cartesian space of a general kinematic tree (KDL::Tree).
*
* @ingroup KinematicFamily
*/
class TreeFkSolverPos_recursive : public TreeFkSolverPos
{
public:
TreeFkSolverPos_recursive(const Tree& tree);
~TreeFkSolverPos_recursive();
virtual int JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName);
private:
const Tree tree;
Frame recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it);
};
}
#endif

62
kdl_install/include/kdl/treeidsolver.hpp Normal file
View File

@@ -0,0 +1,62 @@
// Copyright (C) 2009 Ruben Smits <ruben dot smits at intermodalics dot eu>
// Version: 1.0
// Author: Franco Fusco <franco dot fusco at ls2n dot fr>
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_TREE_IDSOLVER_HPP
#define KDL_TREE_IDSOLVER_HPP
#include "tree.hpp"
#include "frames.hpp"
#include "jntarray.hpp"
#include "solveri.hpp"
namespace KDL
{
typedef std::map<std::string,Wrench> WrenchMap;
/**
* \brief This <strong>abstract</strong> class encapsulates the inverse
* dynamics solver for a KDL::Tree.
*
*/
class TreeIdSolver : public KDL::SolverI
{
public:
/**
* Calculate inverse dynamics, from joint positions, velocity, acceleration, external forces
* to joint torques/forces.
*
* @param q input joint positions
* @param q_dot input joint velocities
* @param q_dotdot input joint accelerations
* @param f_ext the external forces (no gravity) on the segments
* @param torque output joint torques
*
* @return if < 0 something went wrong
*/
virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext,JntArray &torques)=0;
// Need functions to return the manipulator mass, coriolis and gravity matrices - Lagrangian Formulation.
};
}
#endif

84
kdl_install/include/kdl/treeidsolver_recursive_newton_euler.hpp Normal file
View File

@@ -0,0 +1,84 @@
// Copyright (C) 2009 Ruben Smits <ruben dot smits at intermodalics dot eu>
// Version: 1.0
// Author: Franco Fusco <franco dot fusco at ls2n dot fr>
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_TREE_IDSOLVER_RECURSIVE_NEWTON_EULER_HPP
#define KDL_TREE_IDSOLVER_RECURSIVE_NEWTON_EULER_HPP
#include "treeidsolver.hpp"
namespace KDL{
/**
* \brief Recursive newton euler inverse dynamics solver for kinematic trees.
*
* It calculates the torques for the joints, given the motion of
* the joints (q,qdot,qdotdot), external forces on the segments
* (expressed in the segments reference frame) and the dynamical
* parameters of the segments.
*
* This is an extension of the inverse dynamic solver for kinematic chains,
* \see ChainIdSolver_RNE. The main difference is the use of STL maps
* instead of vectors to represent external wrenches (as well as internal
* variables exploited during the recursion).
*/
class TreeIdSolver_RNE : public TreeIdSolver {
public:
/**
* Constructor for the solver, it will allocate all the necessary memory
* \param tree The kinematic tree to calculate the inverse dynamics for, an internal reference will be stored.
* \param grav The gravity vector to use during the calculation.
*/
TreeIdSolver_RNE(const Tree& tree, Vector grav);
/**
* Function to calculate from Cartesian forces to joint torques.
* Input parameters;
* \param q The current joint positions
* \param q_dot The current joint velocities
* \param q_dotdot The current joint accelerations
* \param f_ext The external forces (no gravity) on the segments
* Output parameters:
* \param torques the resulting torques for the joints
*/
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray &torques);
/// @copydoc KDL::SolverI::updateInternalDataStructures
virtual void updateInternalDataStructures();
private:
///Helper function to initialize private members X, S, v, a, f
void initAuxVariables();
///One recursion step
void rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap& f_ext, JntArray& torques);
const Tree& tree;
unsigned int nj;
unsigned int ns;
std::map<std::string,Frame> X;
std::map<std::string,Twist> S;
std::map<std::string,Twist> v;
std::map<std::string,Twist> a;
std::map<std::string,Wrench> f;
Twist ag;
};
}
#endif

77
kdl_install/include/kdl/treeiksolver.hpp Normal file
View File

@@ -0,0 +1,77 @@
/*
* treeiksolver.hpp
*
* Created on: Nov 28, 2008
* Author: rubensmits
*/
#ifndef TREEIKSOLVER_HPP_
#define TREEIKSOLVER_HPP_
#include "tree.hpp"
#include "jntarray.hpp"
#include "frames.hpp"
#include <map>
namespace KDL {
typedef std::map<std::string, Twist> Twists;
typedef std::map<std::string, Jacobian> Jacobians;
typedef std::map<std::string, Frame> Frames;
/**
* \brief This <strong>abstract</strong> class encapsulates the inverse
* position solver for a KDL::Chain.
*
* @ingroup KinematicFamily
*/
class TreeIkSolverPos {
public:
/**
* Calculate inverse position kinematics, from cartesian
*coordinates to joint coordinates.
*
* @param q_init initial guess of the joint coordinates
* @param p_in input cartesian coordinates
* @param q_out output joint coordinates
*
* @return if < 0 something went wrong
* otherwise (>=0) remaining (weighted) distance to target
*/
virtual double CartToJnt(const JntArray& q_init, const Frames& p_in,JntArray& q_out)=0;
virtual ~TreeIkSolverPos() {
}
;
};
/**
* \brief This <strong>abstract</strong> class encapsulates the inverse
* velocity solver for a KDL::Tree.
*
* @ingroup KinematicFamily
*/
class TreeIkSolverVel {
public:
/**
* Calculate inverse velocity kinematics, from joint positions
*and cartesian velocities to joint velocities.
*
* @param q_in input joint positions
* @param v_in input cartesian velocity
* @param qdot_out output joint velocities
*
* @return if < 0 something went wrong
* distance to goal otherwise (weighted norm of v_in)
*/
virtual double CartToJnt(const JntArray& q_in, const Twists& v_in, JntArray& qdot_out)=0;
virtual ~TreeIkSolverVel() {
}
;
};
}
#endif /* TREEIKSOLVER_HPP_ */

84
kdl_install/include/kdl/treeiksolverpos_nr_jl.hpp Normal file
View File

@@ -0,0 +1,84 @@
// Copyright (C) 2007-2008 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Copyright (C) 2008 Mikael Mayer
// Copyright (C) 2008 Julia Jesse
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDLTREEIKSOLVERPOS_NR_JL_HPP
#define KDLTREEIKSOLVERPOS_NR_JL_HPP
#include "treeiksolver.hpp"
#include "treefksolver.hpp"
#include <vector>
#include <string>
namespace KDL {
/**
* Implementation of a general inverse position kinematics
* algorithm based on Newton-Raphson iterations to calculate the
* position transformation from Cartesian to joint space of a general
* KDL::Tree. Takes joint limits into account.
*
* @ingroup KinematicFamily
*/
class TreeIkSolverPos_NR_JL: public TreeIkSolverPos {
public:
/**
* Constructor of the solver, it needs the tree, a forward
* position kinematics solver and an inverse velocity
* kinematics solver for that tree, and a list of the segments you are interested in.
*
* @param tree the tree to calculate the inverse position for
* @param endpoints the list of endpoints you are interested in.
* @param q_max the maximum joint positions
* @param q_min the minimum joint positions
* @param fksolver a forward position kinematics solver
* @param iksolver an inverse velocity kinematics solver
* @param maxiter the maximum Newton-Raphson iterations,
* default: 100
* @param eps the precision for the position, used to end the
* iterations, default: epsilon (defined in kdl.hpp)
*
* @return
*/
TreeIkSolverPos_NR_JL(const Tree& tree, const std::vector<std::string>& endpoints, const JntArray& q_min, const JntArray& q_max, TreeFkSolverPos& fksolver,TreeIkSolverVel& iksolver,unsigned int maxiter=100,double eps=1e-6);
~TreeIkSolverPos_NR_JL();
virtual double CartToJnt(const JntArray& q_init, const Frames& p_in, JntArray& q_out);
private:
const Tree tree;
JntArray q_min;
JntArray q_max;
TreeIkSolverVel& iksolver;
TreeFkSolverPos& fksolver;
JntArray delta_q;
Frames frames;
Twists delta_twists;
std::vector<std::string> endpoints;
unsigned int maxiter;
double eps;
};
}
#endif

108
kdl_install/include/kdl/treeiksolverpos_online.hpp Normal file
View File

@@ -0,0 +1,108 @@
// Copyright (C) 2011 PAL Robotics S.L. All rights reserved.
// Copyright (C) 2007-2008 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Copyright (C) 2008 Mikael Mayer
// Copyright (C) 2008 Julia Jesse
// Version: 1.0
// Author: Marcus Liebhardt
// This class has been derived from the KDL::TreeIkSolverPos_NR_JL class
// by Julia Jesse, Mikael Mayer and Ruben Smits
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDLTREEIKSOLVERPOS_ONLINE_HPP
#define KDLTREEIKSOLVERPOS_ONLINE_HPP
#include <vector>
#include <string>
#include "treeiksolver.hpp"
#include "treefksolver.hpp"
namespace KDL {
/**
* Implementation of a general inverse position kinematics algorithm to calculate the position transformation from
* Cartesian to joint space of a general KDL::Tree. This class has been derived from the TreeIkSolverPos_NR_JL class,
* but was modified for online solving for use in realtime systems. Thus, the calculation is only done once,
* meaning that no iteration is done, because this solver is intended to run at a high frequency.
* It enforces velocity limits in task as well as in joint space. It also takes joint limits into account.
*
* @ingroup KinematicFamily
*/
class TreeIkSolverPos_Online: public TreeIkSolverPos {
public:
/**
* Constructor of the solver, it needs the number of joints of the tree, a list of the endpoints
* you are interested in, the maximum and minimum values you want to enforce and a forward position kinematics
* solver as well as an inverse velocity kinematics solver for the calculations
*
* @param nr_of_jnts number of joints of the tree to calculate the joint positions for
* @param endpoints the list of endpoints you are interested in
* @param q_min the minimum joint positions
* @param q_max the maximum joint positions
* @param q_dot_max the maximum joint velocities
* @param x_dot_trans_max the maximum translational velocity of your endpoints
* @param x_dot_rot_max the maximum rotational velocity of your endpoints
* @param fksolver a forward position kinematics solver
* @param iksolver an inverse velocity kinematics solver
*
* @return
*/
TreeIkSolverPos_Online(const double& nr_of_jnts,
const std::vector<std::string>& endpoints,
const JntArray& q_min,
const JntArray& q_max,
const JntArray& q_dot_max,
const double x_dot_trans_max,
const double x_dot_rot_max,
TreeFkSolverPos& fksolver,
TreeIkSolverVel& iksolver);
~TreeIkSolverPos_Online();
virtual double CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out);
private:
/**
* Scales the class member KDL::JntArray q_dot_, if one (or more) joint velocity exceeds the maximum value.
* Scaling is done proportional to the biggest overshoot among all joint velocities.
*/
void enforceJointVelLimits();
/**
* Scales translational and rotational velocity vectors of the class member KDL::Twist twist_,
* if at least one of both exceeds the maximum value/length.
* Scaling is done proportional to the biggest overshoot among both velocities.
*/
void enforceCartVelLimits();
JntArray q_min_;
JntArray q_max_;
JntArray q_dot_max_;
double x_dot_trans_max_;
double x_dot_rot_max_;
TreeFkSolverPos& fksolver_;
TreeIkSolverVel& iksolver_;
JntArray q_dot_;
Twist twist_;
Frames frames_;
Twists delta_twists_;
};
} // namespace
#endif /* KDLTREEIKSOLVERPOS_ONLINE_HPP */

92
kdl_install/include/kdl/treeiksolvervel_wdls.hpp Normal file
View File

@@ -0,0 +1,92 @@
/*
* TreeIkSolverVel_wdls.hpp
*
* Created on: Nov 28, 2008
* Author: rubensmits
*/
#ifndef TREEIKSOLVERVEL_WDLS_HPP_
#define TREEIKSOLVERVEL_WDLS_HPP_
#include "treeiksolver.hpp"
#include "treejnttojacsolver.hpp"
#include <Eigen/Core>
namespace KDL {
class TreeIkSolverVel_wdls: public TreeIkSolverVel {
public:
static const int E_SVD_FAILED = -100; //! Child SVD failed
TreeIkSolverVel_wdls(const Tree& tree, const std::vector<std::string>& endpoints);
virtual ~TreeIkSolverVel_wdls();
virtual double CartToJnt(const JntArray& q_in, const Twists& v_in, JntArray& qdot_out);
/*
* Set the joint space weighting matrix
*
* @param weight_js joint space weighting symmetric matrix,
* default : identity. M_q : This matrix being used as a
* weight for the norm of the joint space speed it HAS TO BE
* symmetric and positive definite. We can actually deal with
* matrices containing a symmetric and positive definite block
* and 0s otherwise. Taking a diagonal matrix as an example, a
* 0 on the diagonal means that the corresponding joints will
* not contribute to the motion of the system. On the other
* hand, the bigger the value, the most the corresponding
* joint will contribute to the overall motion. The obtained
* solution q_dot will actually minimize the weighted norm
* sqrt(q_dot'*(M_q^-2)*q_dot). In the special case we deal
* with, it does not make sense to invert M_q but what is
* important is the physical meaning of all this : a joint
* that has a zero weight in M_q will not contribute to the
* motion of the system and this is equivalent to saying that
* it gets an infinite weight in the norm computation. For
* more detailed explanation : vincent.padois@upmc.fr
*/
void setWeightJS(const Eigen::MatrixXd& Mq);
const Eigen::MatrixXd& getWeightJS() const {return Wq;}
/*
* Set the task space weighting matrix
*
* @param weight_ts task space weighting symmetric matrix,
* default: identity M_x : This matrix being used as a weight
* for the norm of the error (in terms of task space speed) it
* HAS TO BE symmetric and positive definite. We can actually
* deal with matrices containing a symmetric and positive
* definite block and 0s otherwise. Taking a diagonal matrix
* as an example, a 0 on the diagonal means that the
* corresponding task coordinate will not be taken into
* account (ie the corresponding error can be really big). If
* the rank of the jacobian is equal to the number of task
* space coordinates which do not have a 0 weight in M_x, the
* weighting will actually not impact the results (ie there is
* an exact solution to the velocity inverse kinematics
* problem). In cases without an exact solution, the bigger
* the value, the most the corresponding task coordinate will
* be taken into account (ie the more the corresponding error
* will be reduced). The obtained solution will minimize the
* weighted norm sqrt(|x_dot-Jq_dot|'*(M_x^2)*|x_dot-Jq_dot|).
* For more detailed explanation : vincent.padois@upmc.fr
*/
void setWeightTS(const Eigen::MatrixXd& Mx);
const Eigen::MatrixXd& getWeightTS() const {return Wy;}
void setLambda(const double& lambda);
double getLambda () const {return lambda;}
private:
Tree tree;
TreeJntToJacSolver jnttojacsolver;
Jacobians jacobians;
Eigen::MatrixXd J, Wy, Wq, J_Wq, Wy_J_Wq, U, V, Wy_U, Wq_V;
Eigen::VectorXd t, Wy_t, qdot, tmp, S;
double lambda;
};
}
#endif /* TREEIKSOLVERVEL_WDLS_HPP_ */

38
kdl_install/include/kdl/treejnttojacsolver.hpp Normal file
View File

@@ -0,0 +1,38 @@
/*
* TreeJntToJacSolver.hpp
*
* Created on: Nov 27, 2008
* Author: rubensmits
*/
#ifndef TREEJNTTOJACSOLVER_HPP_
#define TREEJNTTOJACSOLVER_HPP_
#include "tree.hpp"
#include "jacobian.hpp"
#include "jntarray.hpp"
namespace KDL {
class TreeJntToJacSolver {
public:
explicit TreeJntToJacSolver(const Tree& tree);
virtual ~TreeJntToJacSolver();
/*
* Calculate the jacobian for a part of the tree: from a certain segment, given by segmentname to the root.
* The resulting jacobian is expressed in the baseframe of the tree ("root"), the reference point is in the end-segment
*/
int JntToJac(const JntArray& q_in, Jacobian& jac,
const std::string& segmentname);
private:
KDL::Tree tree;
};
}//End of namespace
#endif /* TREEJNTTOJACSOLVER_H_ */

249
kdl_install/include/kdl/utilities/error.h Normal file
View File

@@ -0,0 +1,249 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 error.h
error.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* \file
* Defines the exception classes that can be thrown
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: error.h,v 1.1.1.1.2.2 2003/04/04 15:39:43 pissaris Exp $
* $Name: $
****************************************************************************/
#ifndef ERROR_H_84822 // to make it unique, a random number
#define ERROR_H_84822
#include "utility.h"
#include <string.h>
#include <string>
namespace KDL {
/**
* Base class for errors generated by ORO_Geometry
*/
class Error {
public:
/** Returns a description string describing the error.
* the returned pointer only guaranteed to exists as long as
* the Error object exists.
*/
virtual ~Error() {}
virtual const char* Description() const {return "Unspecified Error\n";}
virtual int GetType() const {return 0;}
};
class Error_IO : public Error {
std::string msg;
int typenr;
public:
Error_IO(const std::string& _msg="Unspecified I/O Error",int typenr=0):msg(_msg) {}
virtual const char* Description() const {return msg.c_str();}
virtual int GetType() const {return typenr;}
};
class Error_BasicIO : public Error_IO {};
class Error_BasicIO_File : public Error_BasicIO {
public:
virtual const char* Description() const {return "Error while reading stream";}
virtual int GetType() const {return 1;}
};
class Error_BasicIO_Exp_Delim : public Error_BasicIO {
public:
virtual const char* Description() const {return "Expected Delimiter not encountered";}
virtual int GetType() const {return 2;}
};
class Error_BasicIO_Not_A_Space : public Error_BasicIO {
public:
virtual const char* Description() const {return "Expected space,tab or newline not encountered";}
virtual int GetType() const {return 3;}
};
class Error_BasicIO_Unexpected : public Error_BasicIO {
public:
virtual const char* Description() const {return "Unexpected character";}
virtual int GetType() const {return 4;}
};
class Error_BasicIO_ToBig : public Error_BasicIO {
public:
virtual const char* Description() const {return "Word that is read out of stream is bigger than maxsize";}
virtual int GetType() const {return 5;}
};
class Error_BasicIO_Not_Opened : public Error_BasicIO {
public:
virtual const char* Description() const {return "File cannot be opened";}
virtual int GetType() const {return 6;}
};
class Error_FrameIO : public Error_IO {};
class Error_Frame_Vector_Unexpected_id : public Error_FrameIO {
public:
virtual const char* Description() const {return "Unexpected identifier, expecting a vector (explicit or ZERO)";}
virtual int GetType() const {return 101;}
};
class Error_Frame_Frame_Unexpected_id : public Error_FrameIO {
public:
virtual const char* Description() const {return "Unexpected identifier, expecting a Frame (explicit or DH)";}
virtual int GetType() const {return 102;}
};
class Error_Frame_Rotation_Unexpected_id : public Error_FrameIO {
public:
virtual const char* Description() const {return "Unexpected identifier, expecting a Rotation (explicit or EULERZYX, EULERZYZ, RPY,ROT,IDENTITY)";}
virtual int GetType() const {return 103;}
};
class Error_ChainIO : public Error {};
class Error_Chain_Unexpected_id : public Error_ChainIO {
public:
virtual const char* Description() const {return "Unexpected identifier, expecting TRANS or ROT";}
virtual int GetType() const {return 201;}
};
//! Error_Redundancy indicates an error that occurred during solving for redundancy.
class Error_RedundancyIO:public Error_IO {};
class Error_Redundancy_Illegal_Resolutiontype : public Error_RedundancyIO {
public:
virtual const char* Description() const {return "Illegal Resolutiontype is used in I/O with ResolutionTask";}
virtual int GetType() const {return 301;}
};
class Error_Redundancy:public Error {};
class Error_Redundancy_Unavoidable : public Error_Redundancy {
public:
virtual const char* Description() const {return "Joint limits cannot be avoided";}
virtual int GetType() const {return 1002;}
};
class Error_Redundancy_Low_Manip: public Error_Redundancy {
public:
virtual const char* Description() const {return "Manipulability is very low";}
virtual int GetType() const {return 1003;}
};
class Error_MotionIO : public Error {};
class Error_MotionIO_Unexpected_MotProf : public Error_MotionIO {
public:
virtual const char* Description() const { return "Wrong keyword while reading motion profile";}
virtual int GetType() const {return 2001;}
};
class Error_MotionIO_Unexpected_Traj : public Error_MotionIO {
public:
virtual const char* Description() const { return "Trajectory type keyword not known";}
virtual int GetType() const {return 2002;}
};
class Error_MotionPlanning : public Error {};
class Error_MotionPlanning_Circle_ToSmall : public Error_MotionPlanning {
public:
virtual const char* Description() const { return "Circle : radius is to small";}
virtual int GetType() const {return 3001;}
};
class Error_MotionPlanning_Circle_No_Plane : public Error_MotionPlanning {
public:
virtual const char* Description() const { return "Circle : Plane for motion is not properly defined";}
virtual int GetType() const {return 3002;}
};
class Error_MotionPlanning_Incompatible: public Error_MotionPlanning {
public:
virtual const char* Description() const { return "Acceleration of a rectangular velocityprofile cannot be used";}
virtual int GetType() const {return 3003;}
};
class Error_MotionPlanning_Not_Feasible: public Error_MotionPlanning {
int reason;
public:
Error_MotionPlanning_Not_Feasible(int _reason):reason(_reason) {}
virtual const char* Description() const {
return "Motion Profile with requested parameters is not feasible";
}
virtual int GetType() const {return 3100+reason;}
};
class Error_MotionPlanning_Not_Applicable: public Error_MotionPlanning {
public:
virtual const char* Description() const { return "Method is not applicable for this derived object";}
virtual int GetType() const {return 3004;}
};
//! Abstract subclass of all errors that can be thrown by Adaptive_Integrator
class Error_Integrator : public Error {};
//! Error_Stepsize_Underflow is thrown if the stepsize becomes to small
class Error_Stepsize_Underflow : public Error_Integrator {
public:
virtual const char* Description() const { return "Stepsize Underflow";}
virtual int GetType() const {return 4001;}
};
//! Error_To_Many_Steps is thrown if the number of steps needed to
//! integrate to the desired accuracy becomes to big.
class Error_To_Many_Steps : public Error_Integrator {
public:
virtual const char* Description() const { return "To many steps"; }
virtual int GetType() const {return 4002;}
};
//! Error_Stepsize_To_Small is thrown if the stepsize becomes to small
class Error_Stepsize_To_Small : public Error_Integrator {
public:
virtual const char* Description() const { return "Stepsize to small"; }
virtual int GetType() const {return 4003;}
};
class Error_Criterium : public Error {};
class Error_Criterium_Unexpected_id: public Error_Criterium {
public:
virtual const char* Description() const { return "Unexpected identifier while reading a criterium"; }
virtual int GetType() const {return 5001;}
};
class Error_Limits : public Error {};
class Error_Limits_Unexpected_id: public Error_Limits {
public:
virtual const char* Description() const { return "Unexpected identifier while reading a jointlimits"; }
virtual int GetType() const {return 6001;}
};
class Error_Not_Implemented: public Error {
public:
virtual const char* Description() const { return "The requested object/method/function is not implemented"; }
virtual int GetType() const {return 7000;}
};
}
#endif

70
kdl_install/include/kdl/utilities/error_stack.h Normal file
View File

@@ -0,0 +1,70 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 error_stack.h
error_stack.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/**
* \file
* \author Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
* \version
* ORO_Geometry V0.2
*
* \par history
* - changed layout of the comments to accommodate doxygen
*/
#ifndef ERROR_STACK_H
#define ERROR_STACK_H
#include "utility.h"
#include "utility_io.h"
#include <string>
namespace KDL {
/*
* \todo
* IOTrace-routines store in static memory, should be in thread-local memory.
* pushes a description of the current routine on the IO-stack trace
*/
void IOTrace(const std::string& description);
//! pops a description of the IO-stack
void IOTracePop();
//! outputs the IO-stack to a stream to provide a better errormessage.
void IOTraceOutput(std::ostream& os);
//! outputs one element of the IO-stack to the buffer (maximally size chars)
//! returns empty string if no elements on the stack.
void IOTracePopStr(char* buffer,int size);
}
#endif

26
kdl_install/include/kdl/utilities/hash_combine.h Normal file
View File

@@ -0,0 +1,26 @@
#ifndef KDL_HASH_COMBINE_H_
#define KDL_HASH_COMBINE_H_
#include <functional>
namespace KDL
{
/**
* @brief Combine hash of object \p v to the \p seed
* @param seed Seed to append the hash of \p v
* @param v Object of which the hash should be appended to the seed
*
* Inspired by:
* @link https://github.com/boostorg/multiprecision/blob/boost-1.79.0/include/boost/multiprecision/detail/hash.hpp#L35-L41
*/
template <class T>
inline void hash_combine(std::size_t& seed, const T& v)
{
std::hash<T> hasher;
seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
}
}
#endif

33
kdl_install/include/kdl/utilities/kdl-config.h Normal file
View File

@@ -0,0 +1,33 @@
/* Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */
/* Version: 1.0 */
/* Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */
/* Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */
/* URL: http://www.orocos.org/kdl */
/* This library 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 2.1 of the License, or (at your option) any later version. */
/* This library 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 */
/* Lesser General Public License for more details. */
/* You should have received a copy of the GNU Lesser General Public */
/* License along with this library; if not, write to the Free Software */
/* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA */
/* Methods are inlined */
#define KDL_INLINE 1
/* Column width that is used form printing frames */
#define KDL_FRAME_WIDTH 12
/* Indices are checked when accessing members of the objects */
#define KDL_INDEX_CHECK 1
/* use KDL implementation for == operator */
#define KDL_USE_EQUAL 1

57
kdl_install/include/kdl/utilities/ldl_solver_eigen.hpp Normal file
View File

@@ -0,0 +1,57 @@
// Copyright (C) 2018 Craig Carignan <craigc at ssl dot umd dot edu>
// Version: 1.0
// Author: Craig Carignan <craigc at ssl dot umd dot edu>
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
// Inverse of a positive definite symmetric matrix times a vector
// based on LDL^T Decomposition
#ifndef LDL_SOLVER_EIGEN_HPP
#define LDL_SOLVER_EIGEN_HPP
#include <Eigen/Core>
#include "../solveri.hpp"
namespace KDL
{
/**
* \brief Solves the system of equations Aq = v for q via LDL decomposition,
* where A is a square positive definite matrix
*
* The algorithm factor A into the product of three matrices LDL^T, where L
* is a lower triangular matrix and D is a diagonal matrix. This allows q
* to be computed without explicitly inverting A. Note that the LDL decomposition
* is a variant of the classical Cholesky Decomposition that does not require
* the computation of square roots.
* Input parameters:
* @param A matrix<double>(nxn)
* @param v vector<double> n
* @param vtmp vector<double> n [temp variable]
* Output parameters:
* @param L matrix<double>(nxn)
* @param D vector<double> n
* @param q vector<double> n
* @return 0 if successful, E_SIZE_MISMATCH if dimensions do not match
* References:
* https://en.wikipedia.org/wiki/Cholesky_decomposition
*/
int ldl_solver_eigen(const Eigen::MatrixXd& A, const Eigen::VectorXd& v, Eigen::MatrixXd& L, Eigen::VectorXd& D, Eigen::VectorXd& vtmp, Eigen::VectorXd& q);
}
#endif

494
kdl_install/include/kdl/utilities/rall1d.h Normal file
View File

@@ -0,0 +1,494 @@
/*****************************************************************************
* \file
* class for automatic differentiation on scalar values and 1st
* derivatives .
*
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par Note
* VC6++ contains a bug, concerning the use of inlined friend functions
* in combination with namespaces. So, try to avoid inlined friend
* functions !
*
* \par History
* - $log$
*
* \par Release
* $Id: rall1d.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
* $Name: $
****************************************************************************/
#ifndef Rall1D_H
#define Rall1D_H
#include <assert.h>
#include "utility.h"
namespace KDL {
/**
* Rall1d contains a value, and its gradient, and defines an algebraic structure on this pair.
* This template class has 3 template parameters :
* - T contains the type of the value.
* - V contains the type of the gradient (can be a vector-like type).
* - S defines a scalar type that can operate on Rall1d. This is the type that
* is used to give back values of Norm() etc.
*
* S is useful when you recurse a Rall1d object into itself to create a 2nd, 3rd, 4th,..
* derivatives. (e.g. Rall1d< Rall1d<double>, Rall1d<double>, double> ).
*
* S is always passed by value.
*
* \par Class Type
* Concrete implementation
*/
template <typename T,typename V=T,typename S=T>
class Rall1d
{
public:
typedef T valuetype;
typedef V gradienttype;
typedef S scalartype;
public :
T t; //!< value
V grad; //!< gradient
public :
INLINE Rall1d():t(),grad() {};
T value() const {
return t;
}
V deriv() const {
return grad;
}
explicit INLINE Rall1d(typename TI<T>::Arg c)
{t=T(c);SetToZero(grad);}
INLINE Rall1d(typename TI<T>::Arg tn, typename TI<V>::Arg afg):t(tn),grad(afg) {}
INLINE Rall1d(const Rall1d<T,V,S>& r):t(r.t),grad(r.grad) {}
//if one defines this constructor, it's better optimized then the
//automatically generated one ( this one set's up a loop to copy
// word by word.
INLINE T& Value() {
return t;
}
INLINE V& Gradient() {
return grad;
}
INLINE static Rall1d<T,V,S> Zero() {
Rall1d<T,V,S> tmp;
SetToZero(tmp);
return tmp;
}
INLINE static Rall1d<T,V,S> Identity() {
Rall1d<T,V,S> tmp;
SetToIdentity(tmp);
return tmp;
}
INLINE Rall1d<T,V,S>& operator =(S c)
{t=c;SetToZero(grad);return *this;}
INLINE Rall1d<T,V,S>& operator =(const Rall1d<T,V,S>& r)
{t=r.t;grad=r.grad;return *this;}
INLINE Rall1d<T,V,S>& operator /=(const Rall1d<T,V,S>& rhs)
{
grad = LinComb(rhs.t,grad,-t,rhs.grad) / (rhs.t*rhs.t);
t /= rhs.t;
return *this;
}
INLINE Rall1d<T,V,S>& operator *=(const Rall1d<T,V,S>& rhs)
{
LinCombR(rhs.t,grad,t,rhs.grad,grad);
t *= rhs.t;
return *this;
}
INLINE Rall1d<T,V,S>& operator +=(const Rall1d<T,V,S>& rhs)
{
grad +=rhs.grad;
t +=rhs.t;
return *this;
}
INLINE Rall1d<T,V,S>& operator -=(const Rall1d<T,V,S>& rhs)
{
grad -= rhs.grad;
t -= rhs.t;
return *this;
}
INLINE Rall1d<T,V,S>& operator /=(S rhs)
{
grad /= rhs;
t /= rhs;
return *this;
}
INLINE Rall1d<T,V,S>& operator *=(S rhs)
{
grad *= rhs;
t *= rhs;
return *this;
}
INLINE Rall1d<T,V,S>& operator +=(S rhs)
{
t += rhs;
return *this;
}
INLINE Rall1d<T,V,S>& operator -=(S rhs)
{
t -= rhs;
return *this;
}
// = operators
/* gives warnings on cygwin
template <class T2,class V2,class S2>
friend INLINE Rall1d<T2,V2,S2> operator /(const Rall1d<T2,V2,S2>& lhs,const Rall1d<T2,V2,S2>& rhs);
friend INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
friend INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs);
friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v);
friend INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s);
friend INLINE Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v);
friend INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s);
friend INLINE Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v);
friend INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s);
friend INLINE Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v);
friend INLINE Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s);
// = Mathematical functions that operate on Rall1d objects
friend INLINE Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m) ;
friend INLINE Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x);
friend INLINE Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
friend INLINE Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x);
friend INLINE Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x);
friend INLINE Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x);
friend INLINE S Norm(const Rall1d<T,V,S>& value) ;
friend INLINE Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg);
friend INLINE Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x);
// = Utility functions to improve performance
friend INLINE Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
const T& beta,const Rall1d<T,V,S>& b );
friend INLINE void LinCombR(S alfa,const Rall1d<T,V,S>& a,
const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result );
// = Setting value of a Rall1d object to 0 or 1
friend INLINE void SetToZero(Rall1d<T,V,S>& value);
friend INLINE void SetToOne(Rall1d<T,V,S>& value);
// = Equality in an eps-interval
friend INLINE bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps);
*/
};
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
{
return Rall1d<T,V,S>(lhs.t/rhs.t,(lhs.grad*rhs.t-lhs.t*rhs.grad)/(rhs.t*rhs.t));
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
{
return Rall1d<T,V,S>(lhs.t*rhs.t,rhs.t*lhs.grad+lhs.t*rhs.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
{
return Rall1d<T,V,S>(lhs.t+rhs.t,lhs.grad+rhs.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& lhs,const Rall1d<T,V,S>& rhs)
{
return Rall1d<T,V,S>(lhs.t-rhs.t,lhs.grad-rhs.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& arg)
{
return Rall1d<T,V,S>(-arg.t,-arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator *(S s,const Rall1d<T,V,S>& v)
{
return Rall1d<T,V,S>(s*v.t,s*v.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator *(const Rall1d<T,V,S>& v,S s)
{
return Rall1d<T,V,S>(v.t*s,v.grad*s);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator +(S s,const Rall1d<T,V,S>& v)
{
return Rall1d<T,V,S>(s+v.t,v.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator +(const Rall1d<T,V,S>& v,S s)
{
return Rall1d<T,V,S>(v.t+s,v.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator -(S s,const Rall1d<T,V,S>& v)
{
return Rall1d<T,V,S>(s-v.t,-v.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator -(const Rall1d<T,V,S>& v,S s)
{
return Rall1d<T,V,S>(v.t-s,v.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator /(S s,const Rall1d<T,V,S>& v)
{
return Rall1d<T,V,S>(s/v.t,(-s*v.grad)/(v.t*v.t));
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> operator /(const Rall1d<T,V,S>& v,S s)
{
return Rall1d<T,V,S>(v.t/s,v.grad/s);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> exp(const Rall1d<T,V,S>& arg)
{
T v;
v= (exp(arg.t));
return Rall1d<T,V,S>(v,v*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> log(const Rall1d<T,V,S>& arg)
{
T v;
v=(log(arg.t));
return Rall1d<T,V,S>(v,arg.grad/arg.t);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> sin(const Rall1d<T,V,S>& arg)
{
T v;
v=(sin(arg.t));
return Rall1d<T,V,S>(v,cos(arg.t)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> cos(const Rall1d<T,V,S>& arg)
{
T v;
v=(cos(arg.t));
return Rall1d<T,V,S>(v,-sin(arg.t)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> tan(const Rall1d<T,V,S>& arg)
{
T v;
v=(tan(arg.t));
return Rall1d<T,V,S>(v,arg.grad/sqr(cos(arg.t)));
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> sinh(const Rall1d<T,V,S>& arg)
{
T v;
v=(sinh(arg.t));
return Rall1d<T,V,S>(v,cosh(arg.t)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> cosh(const Rall1d<T,V,S>& arg)
{
T v;
v=(cosh(arg.t));
return Rall1d<T,V,S>(v,sinh(arg.t)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> sqr(const Rall1d<T,V,S>& arg)
{
T v;
v=(arg.t*arg.t);
return Rall1d<T,V,S>(v,(2.0*arg.t)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> pow(const Rall1d<T,V,S>& arg,double m)
{
T v;
v=(pow(arg.t,m));
return Rall1d<T,V,S>(v,(m*v/arg.t)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> sqrt(const Rall1d<T,V,S>& arg)
{
T v;
v=sqrt(arg.t);
return Rall1d<T,V,S>(v, (0.5/v)*arg.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> atan(const Rall1d<T,V,S>& x)
{
T v;
v=(atan(x.t));
return Rall1d<T,V,S>(v,x.grad/(1.0+sqr(x.t)));
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> hypot(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
{
T v;
v=(hypot(y.t,x.t));
return Rall1d<T,V,S>(v,(x.t/v)*x.grad+(y.t/v)*y.grad);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> asin(const Rall1d<T,V,S>& x)
{
T v;
v=(asin(x.t));
return Rall1d<T,V,S>(v,x.grad/sqrt(1.0-sqr(x.t)));
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> acos(const Rall1d<T,V,S>& x)
{
T v;
v=(acos(x.t));
return Rall1d<T,V,S>(v,-x.grad/sqrt(1.0-sqr(x.t)));
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> abs(const Rall1d<T,V,S>& x)
{
T v;
v=(Sign(x));
return Rall1d<T,V,S>(v*x,v*x.grad);
}
template <class T,class V,class S>
INLINE S Norm(const Rall1d<T,V,S>& value)
{
return Norm(value.t);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> tanh(const Rall1d<T,V,S>& arg)
{
T v(tanh(arg.t));
return Rall1d<T,V,S>(v,arg.grad/sqr(cosh(arg.t)));
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> atan2(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
{
T v(x.t*x.t+y.t*y.t);
return Rall1d<T,V,S>(atan2(y.t,x.t),(x.t*y.grad-y.t*x.grad)/v);
}
template <class T,class V,class S>
INLINE Rall1d<T,V,S> LinComb(S alfa,const Rall1d<T,V,S>& a,
const T& beta,const Rall1d<T,V,S>& b ) {
return Rall1d<T,V,S>(
LinComb(alfa,a.t,beta,b.t),
LinComb(alfa,a.grad,beta,b.grad)
);
}
template <class T,class V,class S>
INLINE void LinCombR(S alfa,const Rall1d<T,V,S>& a,
const T& beta,const Rall1d<T,V,S>& b,Rall1d<T,V,S>& result ) {
LinCombR(alfa, a.t, beta, b.t, result.t);
LinCombR(alfa, a.grad, beta, b.grad, result.grad);
}
template <class T,class V,class S>
INLINE void SetToZero(Rall1d<T,V,S>& value)
{
SetToZero(value.grad);
SetToZero(value.t);
}
template <class T,class V,class S>
INLINE void SetToIdentity(Rall1d<T,V,S>& value)
{
SetToIdentity(value.t);
SetToZero(value.grad);
}
template <class T,class V,class S>
INLINE bool Equal(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x,double eps=epsilon)
{
return (Equal(x.t,y.t,eps)&&Equal(x.grad,y.grad,eps));
}
template <class T,class V,class S>
INLINE bool operator==(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
{
#ifdef KDL_USE_EQUAL
return Equal(y, x);
#else
return (x.t == y.t && x.grad == y.grad);
#endif
}
template <class T,class V,class S>
INLINE bool operator!=(const Rall1d<T,V,S>& y,const Rall1d<T,V,S>& x)
{
return !operator==(y, x);
}
}
#endif

38
kdl_install/include/kdl/utilities/rall1d_io.h Normal file
View File

@@ -0,0 +1,38 @@
/*****************************************************************************
* \file
* provides I/O operations on Rall1d
*
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: rall1d_io.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
* $Name: $
****************************************************************************/
#ifndef Rall_IO_H
#define Rall_IO_H
#include <typeinfo>
#include "utility_io.h"
#include "rall1d.h"
namespace KDL {
template <class T,class V,class S>
inline std::ostream& operator << (std::ostream& os,const Rall1d<T,V,S>& r)
{
os << "Rall1d<" << typeid(T).name() << ", "<< typeid(V).name() << ", " << typeid(S).name() << ">(" << r.t <<"," << r.grad <<")";
return os;
}
}
#endif

556
kdl_install/include/kdl/utilities/rall2d.h Normal file
View File

@@ -0,0 +1,556 @@
/*****************************************************************************
* \file
* class for automatic differentiation on scalar values and 1st
* derivatives and 2nd derivative.
*
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par Note
* VC6++ contains a bug, concerning the use of inlined friend functions
* in combination with namespaces. So, try to avoid inlined friend
* functions !
*
* \par History
* - $log$
*
* \par Release
* $Id: rall2d.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
* $Name: $
****************************************************************************/
#ifndef Rall2D_H
#define Rall2D_H
#include <math.h>
#include <assert.h>
#include "utility.h"
namespace KDL {
/**
* Rall2d contains a value, and its gradient and its 2nd derivative, and defines an algebraic
* structure on this pair.
* This template class has 3 template parameters :
* - T contains the type of the value.
* - V contains the type of the gradient (can be a vector-like type).
* - S defines a scalar type that can operate on Rall1d. This is the type that
* is used to give back values of Norm() etc.
*
* S is useful when you recurse a Rall1d object into itself to create a 2nd, 3rd, 4th,..
* derivatives. (e.g. Rall1d< Rall1d<double>, Rall1d<double>, double> ).
*
* S is always passed by value.
*
* \par Class Type
* Concrete implementation
*/
template <class T,class V=T,class S=T>
class Rall2d
{
public :
T t; //!< value
V d; //!< 1st derivative
V dd; //!< 2nd derivative
public :
// = Constructors
INLINE Rall2d():t(),d(),dd() {};
explicit INLINE Rall2d(typename TI<T>::Arg c)
{t=c;SetToZero(d);SetToZero(dd);}
INLINE Rall2d(typename TI<T>::Arg tn,const V& afg):t(tn),d(afg) {SetToZero(dd);}
INLINE Rall2d(typename TI<T>::Arg tn,const V& afg,const V& afg2):t(tn),d(afg),dd(afg2) {}
// = Copy Constructor
INLINE Rall2d(const Rall2d<T,V,S>& r):t(r.t),d(r.d),dd(r.dd) {}
//if one defines this constructor, it's better optimized then the
//automatically generated one ( that one set's up a loop to copy
// word by word.
// = Member functions to access internal structures :
INLINE T& Value() {
return t;
}
INLINE V& D() {
return d;
}
INLINE V& DD() {
return dd;
}
INLINE static Rall2d<T,V,S> Zero() {
Rall2d<T,V,S> tmp;
SetToZero(tmp);
return tmp;
}
INLINE static Rall2d<T,V,S> Identity() {
Rall2d<T,V,S> tmp;
SetToIdentity(tmp);
return tmp;
}
// = assignment operators
INLINE Rall2d<T,V,S>& operator =(S c)
{t=c;SetToZero(d);SetToZero(dd);return *this;}
INLINE Rall2d<T,V,S>& operator =(const Rall2d<T,V,S>& r)
{t=r.t;d=r.d;dd=r.dd;return *this;}
INLINE Rall2d<T,V,S>& operator /=(const Rall2d<T,V,S>& rhs)
{
t /= rhs.t;
d = (d-t*rhs.d)/rhs.t;
dd= (dd - S(2)*d*rhs.d-t*rhs.dd)/rhs.t;
return *this;
}
INLINE Rall2d<T,V,S>& operator *=(const Rall2d<T,V,S>& rhs)
{
t *= rhs.t;
d = (d*rhs.t+t*rhs.d);
dd = (dd*rhs.t+S(2)*d*rhs.d+t*rhs.dd);
return *this;
}
INLINE Rall2d<T,V,S>& operator +=(const Rall2d<T,V,S>& rhs)
{
t +=rhs.t;
d +=rhs.d;
dd+=rhs.dd;
return *this;
}
INLINE Rall2d<T,V,S>& operator -=(const Rall2d<T,V,S>& rhs)
{
t -= rhs.t;
d -= rhs.d;
dd -= rhs.dd;
return *this;
}
INLINE Rall2d<T,V,S>& operator /=(S rhs)
{
t /= rhs;
d /= rhs;
dd /= rhs;
return *this;
}
INLINE Rall2d<T,V,S>& operator *=(S rhs)
{
t *= rhs;
d *= rhs;
dd *= rhs;
return *this;
}
INLINE Rall2d<T,V,S>& operator -=(S rhs)
{
t -= rhs;
return *this;
}
INLINE Rall2d<T,V,S>& operator +=(S rhs)
{
t += rhs;
return *this;
}
// = Operators between Rall2d objects
/*
friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs);
friend INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v);
friend INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s);
friend INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v);
friend INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s);
friend INLINE Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v);
friend INLINE INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s);
friend INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& v);
friend INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s);
// = Mathematical functions that operate on Rall2d objects
friend INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m) ;
friend INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg);
friend INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x);
friend INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
friend INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x);
friend INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x);
// returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
friend INLINE S Norm(const Rall2d<T,V,S>& value) ;
// returns Norm( value.Value() ).
// = Some utility functions to improve performance
// (should also be declared on primitive types to improve uniformity
friend INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
TI<T>::Arg beta,const Rall2d<T,V,S>& b );
friend INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a,
TI<T>::Arg beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result );
// = Setting value of a Rall2d object to 0 or 1
friend INLINE void SetToZero(Rall2d<T,V,S>& value);
friend INLINE void SetToOne(Rall2d<T,V,S>& value);
// = Equality in an eps-interval
friend INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps);
*/
};
// = Operators between Rall2d objects
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
{
Rall2d<T,V,S> tmp;
tmp.t = lhs.t/rhs.t;
tmp.d = (lhs.d-tmp.t*rhs.d)/rhs.t;
tmp.dd= (lhs.dd-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
{
Rall2d<T,V,S> tmp;
tmp.t = lhs.t*rhs.t;
tmp.d = (lhs.d*rhs.t+lhs.t*rhs.d);
tmp.dd = (lhs.dd*rhs.t+S(2)*lhs.d*rhs.d+lhs.t*rhs.dd);
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
{
return Rall2d<T,V,S>(lhs.t+rhs.t,lhs.d+rhs.d,lhs.dd+rhs.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& lhs,const Rall2d<T,V,S>& rhs)
{
return Rall2d<T,V,S>(lhs.t-rhs.t,lhs.d-rhs.d,lhs.dd-rhs.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& arg)
{
return Rall2d<T,V,S>(-arg.t,-arg.d,-arg.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator *(S s,const Rall2d<T,V,S>& v)
{
return Rall2d<T,V,S>(s*v.t,s*v.d,s*v.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator *(const Rall2d<T,V,S>& v,S s)
{
return Rall2d<T,V,S>(v.t*s,v.d*s,v.dd*s);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator +(S s,const Rall2d<T,V,S>& v)
{
return Rall2d<T,V,S>(s+v.t,v.d,v.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator +(const Rall2d<T,V,S>& v,S s)
{
return Rall2d<T,V,S>(v.t+s,v.d,v.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator -(S s,const Rall2d<T,V,S>& v)
{
return Rall2d<T,V,S>(s-v.t,-v.d,-v.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator -(const Rall2d<T,V,S>& v,S s)
{
return Rall2d<T,V,S>(v.t-s,v.d,v.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator /(S s,const Rall2d<T,V,S>& rhs)
{
Rall2d<T,V,S> tmp;
tmp.t = s/rhs.t;
tmp.d = (-tmp.t*rhs.d)/rhs.t;
tmp.dd= (-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> operator /(const Rall2d<T,V,S>& v,S s)
{
return Rall2d<T,V,S>(v.t/s,v.d/s,v.dd/s);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> exp(const Rall2d<T,V,S>& arg)
{
Rall2d<T,V,S> tmp;
tmp.t = exp(arg.t);
tmp.d = tmp.t*arg.d;
tmp.dd = tmp.d*arg.d+tmp.t*arg.dd;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> log(const Rall2d<T,V,S>& arg)
{
Rall2d<T,V,S> tmp;
tmp.t = log(arg.t);
tmp.d = arg.d/arg.t;
tmp.dd = (arg.dd-tmp.d*arg.d)/arg.t;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> sin(const Rall2d<T,V,S>& arg)
{
T v1 = sin(arg.t);
T v2 = cos(arg.t);
return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd - (v1*arg.d)*arg.d );
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> cos(const Rall2d<T,V,S>& arg)
{
T v1 = cos(arg.t);
T v2 = -sin(arg.t);
return Rall2d<T,V,S>(v1,v2*arg.d, v2*arg.dd - (v1*arg.d)*arg.d);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> tan(const Rall2d<T,V,S>& arg)
{
T v1 = tan(arg.t);
T v2 = S(1)+sqr(v1);
return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd+(S(2)*v1*sqr(arg.d))));
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> sinh(const Rall2d<T,V,S>& arg)
{
T v1 = sinh(arg.t);
T v2 = cosh(arg.t);
return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> cosh(const Rall2d<T,V,S>& arg)
{
T v1 = cosh(arg.t);
T v2 = sinh(arg.t);
return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> tanh(const Rall2d<T,V,S>& arg)
{
T v1 = tanh(arg.t);
T v2 = S(1)-sqr(v1);
return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd-(S(2)*v1*sqr(arg.d))));
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> sqr(const Rall2d<T,V,S>& arg)
{
return Rall2d<T,V,S>(arg.t*arg.t,
(S(2)*arg.t)*arg.d,
S(2)*(sqr(arg.d)+arg.t*arg.dd)
);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> pow(const Rall2d<T,V,S>& arg,double m)
{
Rall2d<T,V,S> tmp;
tmp.t = pow(arg.t,m);
T v2 = (m/arg.t)*tmp.t;
tmp.d = v2*arg.d;
tmp.dd = (S((m-1))/arg.t)*tmp.d*arg.d + v2*arg.dd;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> sqrt(const Rall2d<T,V,S>& arg)
{
/* By inversion of sqr(x) :*/
Rall2d<T,V,S> tmp;
tmp.t = sqrt(arg.t);
tmp.d = (S(0.5)/tmp.t)*arg.d;
tmp.dd = (S(0.5)*arg.dd-sqr(tmp.d))/tmp.t;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> asin(const Rall2d<T,V,S>& arg)
{
/* By inversion of sin(x) */
Rall2d<T,V,S> tmp;
tmp.t = asin(arg.t);
T v = cos(tmp.t);
tmp.d = arg.d/v;
tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> acos(const Rall2d<T,V,S>& arg)
{
/* By inversion of cos(x) */
Rall2d<T,V,S> tmp;
tmp.t = acos(arg.t);
T v = -sin(tmp.t);
tmp.d = arg.d/v;
tmp.dd = (arg.dd+arg.t*sqr(tmp.d))/v;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> atan(const Rall2d<T,V,S>& x)
{
/* By inversion of tan(x) */
Rall2d<T,V,S> tmp;
tmp.t = atan(x.t);
T v = S(1)+sqr(x.t);
tmp.d = x.d/v;
tmp.dd = x.dd/v-(S(2)*x.t)*sqr(tmp.d);
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> atan2(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
{
Rall2d<T,V,S> tmp;
tmp.t = atan2(y.t,x.t);
T v = sqr(y.t)+sqr(x.t);
tmp.d = (x.t*y.d-x.d*y.t)/v;
tmp.dd = ( x.t*y.dd-x.dd*y.t-S(2)*(x.t*x.d+y.t*y.d)*tmp.d ) / v;
return tmp;
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> abs(const Rall2d<T,V,S>& x)
{
T v(Sign(x));
return Rall2d<T,V,S>(v*x,v*x.d,v*x.dd);
}
template <class T,class V,class S>
INLINE Rall2d<T,V,S> hypot(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
{
Rall2d<T,V,S> tmp;
tmp.t = hypot(y.t,x.t);
tmp.d = (x.t*x.d+y.t*y.d)/tmp.t;
tmp.dd = (sqr(x.d)+x.t*x.dd+sqr(y.d)+y.t*y.dd-sqr(tmp.d))/tmp.t;
return tmp;
}
// returns sqrt(y*y+x*x), but is optimized for accuracy and speed.
template <class T,class V,class S>
INLINE S Norm(const Rall2d<T,V,S>& value)
{
return Norm(value.t);
}
// returns Norm( value.Value() ).
// (should also be declared on primitive types to improve uniformity
template <class T,class V,class S>
INLINE Rall2d<T,V,S> LinComb(S alfa,const Rall2d<T,V,S>& a,
const T& beta,const Rall2d<T,V,S>& b ) {
return Rall2d<T,V,S>(
LinComb(alfa,a.t,beta,b.t),
LinComb(alfa,a.d,beta,b.d),
LinComb(alfa,a.dd,beta,b.dd)
);
}
template <class T,class V,class S>
INLINE void LinCombR(S alfa,const Rall2d<T,V,S>& a,
const T& beta,const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result ) {
LinCombR(alfa, a.t, beta, b.t, result.t);
LinCombR(alfa, a.d, beta, b.d, result.d);
LinCombR(alfa, a.dd, beta, b.dd, result.dd);
}
template <class T,class V,class S>
INLINE void SetToZero(Rall2d<T,V,S>& value)
{
SetToZero(value.t);
SetToZero(value.d);
SetToZero(value.dd);
}
template <class T,class V,class S>
INLINE void SetToIdentity(Rall2d<T,V,S>& value)
{
SetToZero(value.d);
SetToIdentity(value.t);
SetToZero(value.dd);
}
template <class T,class V,class S>
INLINE bool Equal(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x,double eps=epsilon)
{
return (Equal(x.t,y.t,eps)&&
Equal(x.d,y.d,eps)&&
Equal(x.dd,y.dd,eps)
);
}
template <class T,class V,class S>
INLINE bool operator==(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
{
#ifdef KDL_USE_EQUAL
return Equal(y, x);
#else
return (x.t == y.t &&
x.d == y.d &&
x.dd == y.dd);
#endif
}
template <class T,class V,class S>
INLINE bool operator!=(const Rall2d<T,V,S>& y,const Rall2d<T,V,S>& x)
{
return !operator==(y, x);
}
}
#endif

38
kdl_install/include/kdl/utilities/rall2d_io.h Normal file
View File

@@ -0,0 +1,38 @@
/*****************************************************************************
* \file
* provides I/O operations on Rall1d
*
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: rall2d_io.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
* $Name: $
****************************************************************************/
#ifndef Rall2d_IO_H
#define Rall2d_IO_H
#include <typeinfo>
#include "utility_io.h"
#include "rall2d.h"
namespace KDL {
template <class T,class V,class S>
std::ostream& operator << (std::ostream& os,const Rall2d<T,V,S>& r)
{
os << "Rall2d<" << typeid(T).name() << ", "<< typeid(V).name() << ", " << typeid(S).name() << ">(" << r.t <<"," << r.d <<","<<r.dd<<")";
return os;
}
}
#endif

101
kdl_install/include/kdl/utilities/rallNd.h Normal file
View File

@@ -0,0 +1,101 @@
#ifndef RALLND_H
#define RALLND_H
#include "rall1d.h"
#include "rall1d_io.h"
#include "rall2d.h"
#include "rall2d_io.h"
/**
* The Rall1d class allows for a 24-line implementation of rall numbers
* generalized to the Nth derivative !
* The efficiency is not very good for high derivatives.
* This could be improved by also using Rall2d
*
template <int N>
class RallNd :
public Rall1d< RallNd<N-1>, RallNd<N-1>, double >
{
public:
RallNd() {}
RallNd(const Rall1d< RallNd<N-1>, RallNd<N-1>,double>& arg) :
Rall1d< RallNd<N-1>, RallNd<N-1>,double>(arg) {}
RallNd(double value,double d[]) {
this->t = RallNd<N-1>(value,d);
this->grad = RallNd<N-1>(d[0],&d[1]);
}
};
template <>
class RallNd<1> : public Rall1d<double> {
public:
RallNd() {}
RallNd(const Rall1d<double>& arg) :
Rall1d<double,double,double>(arg) {}
RallNd(double value,double d[]) {
t = value;
grad = d[0];
}
};
*/
/**
* to be checked..
*/
/**
* Als je tot 3de orde een efficiente berekening kan doen,
* dan kan je tot een willekeurige orde alles efficient berekenen
* 0 1 2 3
* ==> 1 2 3 4
* ==> 3 4 5 6
* 4 5 6 7
*
* de aangeduide berekeningen zijn niet noodzakelijk, en er is dan niets
* verniet berekend in de recursieve implementatie.
* of met 2de orde over 1ste order : kan ook efficient :
* 0 1
* ==>1 2
* 2 3
*/
// N>2:
template <int N>
class RallNd :
public Rall2d< RallNd<N-2>, RallNd<N-2>, double >
{
public:
RallNd() {}
RallNd(const Rall2d< RallNd<N-2>, RallNd<N-2>,double>& arg) :
Rall2d< RallNd<N-2>, RallNd<N-2>,double>(arg) {}
RallNd(double value,double d[]) {
this->t = RallNd<N-2>(value,d); // 0 1 2
this->d = RallNd<N-2>(d[0],&d[1]); // 1 2 3 iseigenlijk niet nodig
this->dd = RallNd<N-2>(d[1],&d[2]); // 2 3 4
}
};
template <>
class RallNd<2> : public Rall2d<double> {
public:
RallNd() {}* (dwz. met evenveel numerieke operaties als een
RallNd(const Rall2d<double>& arg) :
Rall2d<double>(arg) {}
RallNd(double value,double d[]) {
t = value;
d = d[0];
dd= d[1];
}
};
template <>
class RallNd<1> : public Rall1d<double> {
public:
RallNd() {}
RallNd(const Rall1d<double>& arg) :
Rall1d<double>(arg) {}
RallNd(double value,double d[]) {
t = value;
grad = d[0];
}
};
#endif

78
kdl_install/include/kdl/utilities/scoped_ptr.hpp Normal file
View File

@@ -0,0 +1,78 @@
/***************************************************************************
scoped_ptr - a not too smart pointer for exception safe heap objects
-------------------------
begin : May 2019
copyright : (C) 2019 Intermodalics
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
#if (__cplusplus > 199711L)
#include <utility>
#else
#include <algorithm>
#endif
namespace KDL {
template<typename T> class scoped_ptr;
template<typename T> void swap(scoped_ptr<T>&, scoped_ptr<T>&);
template<typename T>
class scoped_ptr {
public:
scoped_ptr() : ptr_(0) { }
explicit scoped_ptr(T* p) : ptr_(p) { }
~scoped_ptr() { delete ptr_; }
T* operator->() { return ptr_; }
const T* operator->() const { return ptr_; }
T* get() const { return ptr_; }
void reset(T* p = 0) {
T* old = ptr_;
ptr_ = p;
delete old;
}
T* release() {
T* old = ptr_;
ptr_ = 0;
return old;
}
friend void swap<>(scoped_ptr<T>& a, scoped_ptr<T>& b);
private:
scoped_ptr(const scoped_ptr&); // not-copyable
scoped_ptr& operator=(const scoped_ptr&); // not-copyable
T* ptr_;
};
template<typename T>
void swap(scoped_ptr<T>& a, scoped_ptr<T>& b) {
using std::swap;
swap(a.ptr_, b.ptr_);
}
} // namespace KDL

46
kdl_install/include/kdl/utilities/svd_HH.hpp Normal file
View File

@@ -0,0 +1,46 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_SVD_HH_HPP
#define KDL_SVD_HH_HPP
#include "../jacobian.hpp"
#include "../jntarray.hpp"
#include <vector>
namespace KDL
{
class SVD_HH
{
public:
SVD_HH(const Jacobian& jac);
~SVD_HH();
int calculate(const Jacobian& jac,std::vector<JntArray>& U,
JntArray& w,std::vector<JntArray>& v,int maxiter);
private:
JntArray tmp;
};
}
#endif

69
kdl_install/include/kdl/utilities/svd_eigen_HH.hpp Normal file
View File

@@ -0,0 +1,69 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
//Based on the svd of the KDL-0.2 library by Erwin Aertbelien
#ifndef SVD_EIGEN_HH_HPP
#define SVD_EIGEN_HH_HPP
#include <Eigen/Core>
#include <algorithm>
namespace KDL
{
inline double PYTHAG(double a,double b) {
double at,bt,ct;
at = fabs(a);
bt = fabs(b);
if (at > bt ) {
ct=bt/at;
return at*sqrt(1.0+ct*ct);
} else {
if (bt==0)
return 0.0;
else {
ct=at/bt;
return bt*sqrt(1.0+ct*ct);
}
}
}
inline double SIGN(double a,double b) {
return ((b) >= 0.0 ? fabs(a) : -fabs(a));
}
/**
* svd calculation of eigen matrices
*
* @param A matrix<double>(mxn)
* @param U matrix<double>(mxn)
* @param S vector<double> n
* @param V matrix<double>(nxn)
* @param tmp vector<double> n
* @param maxiter defaults to 150
*
* @return -2 if maxiter exceeded, 0 otherwise
*/
int svd_eigen_HH(const Eigen::MatrixXd& A,Eigen::MatrixXd& U,Eigen::VectorXd& S,Eigen::MatrixXd& V,Eigen::VectorXd& tmp,int maxiter=150,double epsilon=1e-300);
}
#endif

64
kdl_install/include/kdl/utilities/svd_eigen_Macie.hpp Normal file
View File

@@ -0,0 +1,64 @@
// Copyright (C) 2008 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library 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 2.1 of the License, or (at your option) any later version.
// This library 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
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
//implementation of svd according to (Maciejewski and Klein,1989)
//and (Braun, Ulrey, Maciejewski and Siegel,2002)
/**
* \file svd_eigen_Macie.hpp
* provides Maciejewski's implementation for SVD.
*/
#ifndef SVD_EIGEN_MACIE
#define SVD_EIGEN_MACIE
#include <Eigen/Core>
namespace KDL
{
/**
* svd_eigen_Macie provides Maciejewski implementation for SVD.
*
* computes the singular value decomposition of a matrix A, such that
* A=U*Sm*V
*
* (Maciejewski and Klein,1989) and (Braun, Ulrey, Maciejewski and Siegel,2002)
*
* \param A [INPUT] is an \f$m \times n\f$-matrix, where \f$ m \geq n \f$.
* \param S [OUTPUT] is an \f$n\f$-vector, representing the diagonal elements of the diagonal matrix Sm.
* \param U [INPUT/OUTPUT] is an \f$m \times m\f$ orthonormal matrix.
* \param V [INPUT/OUTPUT] is an \f$n \times n\f$ orthonormal matrix.
* \param B [TEMPORARY] is an \f$m \times n\f$ matrix used for temporary storage.
* \param tempi [TEMPORARY] is an \f$m\f$ vector used for temporary storage.
* \param threshold [INPUT] Threshold to determine orthogonality.
* \param toggle [INPUT] toggle this boolean variable on each call of this routine.
* \return number of sweeps.
*/
int svd_eigen_Macie(const Eigen::MatrixXd& A,Eigen::MatrixXd& U,Eigen::VectorXd& S, Eigen::MatrixXd& V,
Eigen::MatrixXd& B, Eigen::VectorXd& tempi,
double threshold,bool toggle);
}
#endif

111
kdl_install/include/kdl/utilities/traits.h Normal file
View File

@@ -0,0 +1,111 @@
#ifndef KDLPV_TRAITS_H
#define KDLPV_TRAITS_H
#include "utility.h"
// forwards declarations :
namespace KDL {
class Frame;
class Rotation;
class Vector;
class Twist;
class Wrench;
class FrameVel;
class RotationVel;
class VectorVel;
class TwistVel;
}
/**
* @brief Traits are traits classes to determine the type of a derivative of another type.
*
* For geometric objects the "geometric" derivative is chosen. For example the derivative of a Rotation
* matrix is NOT a 3x3 matrix containing the derivative of the elements of a rotation matrix. The derivative
* of the rotation matrix is a Vector corresponding the rotational velocity. Mostly used in template classes
* and routines to derive a correct type when needed.
*
* You can see this as a compile-time lookuptable to find the type of the derivative.
*
* Example
* \verbatim
Rotation R;
Traits<Rotation> dR;
\endverbatim
*/
template <typename T>
struct Traits {
typedef T valueType;
typedef T derivType;
};
template <>
struct Traits<KDL::Frame> {
typedef KDL::Frame valueType;
typedef KDL::Twist derivType;
};
template <>
struct Traits<KDL::Twist> {
typedef KDL::Twist valueType;
typedef KDL::Twist derivType;
};
template <>
struct Traits<KDL::Wrench> {
typedef KDL::Wrench valueType;
typedef KDL::Wrench derivType;
};
template <>
struct Traits<KDL::Rotation> {
typedef KDL::Rotation valueType;
typedef KDL::Vector derivType;
};
template <>
struct Traits<KDL::Vector> {
typedef KDL::Vector valueType;
typedef KDL::Vector derivType;
};
template <>
struct Traits<double> {
typedef double valueType;
typedef double derivType;
};
template <>
struct Traits<float> {
typedef float valueType;
typedef float derivType;
};
template <>
struct Traits<KDL::FrameVel> {
typedef KDL::Frame valueType;
typedef KDL::TwistVel derivType;
};
template <>
struct Traits<KDL::TwistVel> {
typedef KDL::Twist valueType;
typedef KDL::TwistVel derivType;
};
template <>
struct Traits<KDL::RotationVel> {
typedef KDL::Rotation valueType;
typedef KDL::VectorVel derivType;
};
template <>
struct Traits<KDL::VectorVel> {
typedef KDL::Vector valueType;
typedef KDL::VectorVel derivType;
};
#endif

301
kdl_install/include/kdl/utilities/utility.h Normal file
View File

@@ -0,0 +1,301 @@
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: utility.h,v 1.1.1.1.2.4 2003/07/18 14:58:36 psoetens Exp $
* $Name: $
* \file
* Included by most lrl-files to provide some general
* functions and macro definitions.
*
* \par history
* - changed layout of the comments to accommodate doxygen
*/
#ifndef KDL_UTILITY_H
#define KDL_UTILITY_H
#include "kdl-config.h"
#include <cstdlib>
#include <cassert>
#include <cmath>
/////////////////////////////////////////////////////////////
// configurable options for the frames library.
#ifdef KDL_INLINE
#ifdef _MSC_VER
// Microsoft Visual C
#define IMETHOD __forceinline
#else
// Some other compiler, e.g. gcc
#define IMETHOD inline
#endif
#else
#define IMETHOD
#endif
//! turn on or off frames bounds checking. If turned on, assert() can still
//! be turned off with -DNDEBUG.
#ifdef KDL_INDEX_CHECK
#define FRAMES_CHECKI(a) assert(a)
#else
#define FRAMES_CHECKI(a)
#endif
namespace KDL {
#ifdef __GNUC__
// so that sin,cos can be overloaded and complete
// resolution of overloaded functions work.
using ::sin;
using ::cos;
using ::exp;
using ::log;
using ::sin;
using ::cos;
using ::tan;
using ::sinh;
using ::cosh;
using ::pow;
using ::sqrt;
using ::atan;
using ::hypot;
using ::asin;
using ::acos;
using ::tanh;
using ::atan2;
#endif
#ifndef __GNUC__
//only real solution : get Rall1d and varia out of namespaces.
#pragma warning (disable:4786)
inline double sin(double a) {
return ::sin(a);
}
inline double cos(double a) {
return ::cos(a);
}
inline double exp(double a) {
return ::exp(a);
}
inline double log(double a) {
return ::log(a);
}
inline double tan(double a) {
return ::tan(a);
}
inline double cosh(double a) {
return ::cosh(a);
}
inline double sinh(double a) {
return ::sinh(a);
}
inline double sqrt(double a) {
return ::sqrt(a);
}
inline double atan(double a) {
return ::atan(a);
}
inline double acos(double a) {
return ::acos(a);
}
inline double asin(double a) {
return ::asin(a);
}
inline double tanh(double a) {
return ::tanh(a);
}
inline double pow(double a,double b) {
return ::pow(a,b);
}
inline double atan2(double a,double b) {
return ::atan2(a,b);
}
#endif
#if (__cplusplus > 199711L)
using std::isnan;
#endif
/**
* Auxiliary class for argument types (Trait-template class )
*
* Is used to pass doubles by value, and arbitrary objects by const reference.
* This is TWICE as fast (2 x less memory access) and avoids bugs in VC6++ concerning
* the assignment of the result of intrinsic functions to const double&-typed variables,
* and optimization on.
*/
template <class T>
class TI
{
public:
typedef const T& Arg; //!< Arg is used for passing the element to a function.
};
template <>
class TI<double> {
public:
typedef double Arg;
};
template <>
class TI<int> {
public:
typedef int Arg;
};
/**
* /note linkage
* Something fishy about the difference between C++ and C
* in C++ const values default to INTERNAL linkage, in C they default
* to EXTERNAL linkage. Here the constants should have EXTERNAL linkage
* because they, for at least some of them, can be changed by the user.
* If you want to explicitly declare internal linkage, use "static".
*/
//!
extern int STREAMBUFFERSIZE;
//! maximal length of a file name
extern int MAXLENFILENAME;
//! the value of pi
extern const double PI;
//! the value of pi/2
extern const double PI_2;
//! the value of pi/4
extern const double PI_4;
//! the value pi/180
extern const double deg2rad;
//! the value 180/pi
extern const double rad2deg;
//! default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
extern double epsilon;
//! the number of derivatives used in the RN-... objects.
extern int VSIZE;
#ifndef _MFC_VER
#undef max
inline double max(double a,double b) {
if (b<a)
return a;
else
return b;
}
#undef min
inline double min(double a,double b) {
if (b<a)
return b;
else
return a;
}
#endif
#ifdef _MSC_VER
//#pragma inline_depth( 255 )
//#pragma inline_recursion( on )
#define INLINE __forceinline
//#define INLINE inline
#else
#define INLINE inline
#endif
inline double LinComb(double alfa,double a,
double beta,double b ) {
return alfa*a+beta*b;
}
inline void LinCombR(double alfa,double a,
double beta,double b,double& result ) {
result=alfa*a+beta*b;
}
//! to uniformly set double, RNDouble,Vector,... objects to zero in template-classes
inline void SetToZero(double& arg) {
arg=0;
}
//! to uniformly set double, RNDouble,Vector,... objects to the identity element in template-classes
inline void SetToIdentity(double& arg) {
arg=1;
}
inline double sign(double arg) {
return (arg<0)?(-1):(1);
}
inline double sqr(double arg) { return arg*arg;}
inline double Norm(double arg) {
return fabs( (double)arg );
}
#if defined __WIN32__ && !defined __GNUC__
inline double hypot(double y,double x) { return ::_hypot(y,x);}
inline double abs(double x) { return ::fabs(x);}
#endif
// compares whether 2 doubles are equal in an eps-interval.
// Does not check whether a or b represents numbers
// On VC6, if a/b is -INF, it returns false;
inline bool Equal(double a,double b,double eps=epsilon)
{
double tmp=(a-b);
return ((eps>tmp)&& (tmp>-eps) );
}
inline void random(double& a) {
a = 1.98*rand()/(double)RAND_MAX -0.99;
}
inline void posrandom(double& a) {
a = 0.001+0.99*rand()/(double)RAND_MAX;
}
inline double diff(double a,double b,double dt) {
return (b-a)/dt;
}
//inline float diff(float a,float b,double dt) {
//return (b-a)/dt;
//}
inline double addDelta(double a,double da,double dt) {
return a+da*dt;
}
//inline float addDelta(float a,float da,double dt) {
// return a+da*dt;
//}
}
#endif

79
kdl_install/include/kdl/utilities/utility_io.h Normal file
View File

@@ -0,0 +1,79 @@
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: utility_io.h,v 1.1.1.1.2.3 2003/06/26 15:23:59 psoetens Exp $
* $Name: $
*
* \file utility_io.h
* Included by most lrl-files to provide some general
* functions and macro definitions related to file/stream I/O.
*/
#ifndef KDL_UTILITY_IO_H_84822
#define KDL_UTILITY_IO_H_84822
//#include <kdl/kdl-config.h>
// Standard includes
#include <iostream>
#include <iomanip>
#include <fstream>
namespace KDL {
/**
* checks validity of basic io of is
*/
void _check_istream(std::istream& is);
/**
* Eats characters of the stream until the character delim is encountered
* @param is a stream
* @param delim eat until this character is encountered
*/
void Eat(std::istream& is, int delim );
/**
* Eats characters of the stream as long as they satisfy the description in descript
* @param is a stream
* @param descript description string. A sequence of spaces, tabs,
* new-lines and comments is regarded as 1 space in the description string.
*/
void Eat(std::istream& is,const char* descript);
/**
* Eats a word of the stream delimited by the letters in delim or space(tabs...)
* @param is a stream
* @param delim a string containing the delimmiting characters
* @param storage for returning the word
* @param maxsize a word can be maximally maxsize-1 long.
*/
void EatWord(std::istream& is,const char* delim,char* storage,int maxsize);
/**
* Eats characters of the stream until the character delim is encountered
* similar to Eat(is,delim) but spaces at the end are not read.
* @param is a stream
* @param delim eat until this character is encountered
*/
void EatEnd( std::istream& is, int delim );
}
#endif

107
kdl_install/include/kdl/velocityprofile.hpp Normal file
View File

@@ -0,0 +1,107 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 velocityprofile.h
velocityprofile.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
*
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: velocityprofile.h,v 1.1.1.1.2.5 2003/07/24 13:26:15 psoetens Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_VELOCITYPROFILE_H
#define KDL_VELOCITYPROFILE_H
#include "utilities/utility.h"
#include "utilities/utility_io.h"
namespace KDL {
/**
* A VelocityProfile stores the velocity profile that
* is used within a trajectory. A velocity profile is the function that
* expresses position, velocity and acceleration of a point on a curve
* in function of time. It defines the how a point s moves on a path S.
* @ingroup Motion
*/
class VelocityProfile
{
public:
// trajectory parameters are set in constructor of
// derived class
virtual void SetProfile(double pos1,double pos2) = 0;
// sets a trajectory from pos1 to pos2 as fast as possible
virtual void SetProfileDuration(
double pos1,double pos2,double duration) = 0;
// Sets a trajectory from pos1 to pos2 in <duration> seconds.
// @post new.Duration() will not be shorter than the one obtained
// from SetProfile(pos1,pos2).
virtual double Duration() const = 0;
// returns the duration of the motion in [sec]
virtual double Pos(double time) const = 0;
// returns the position at <time> in the units of the input
// of the constructor of the derived class.
virtual double Vel(double time) const = 0;
// returns the velocity at <time> in the units of the input
// of the constructor of the derived class.
virtual double Acc(double time) const = 0;
// returns the acceleration at <time> in the units of the input
// of the constructor of the derived class.
virtual void Write(std::ostream& os) const = 0;
// Writes object to a stream.
static VelocityProfile* Read(std::istream& is);
// reads a VelocityProfile object from the stream and returns it.
virtual VelocityProfile* Clone() const = 0;
// returns copy of current VelocityProfile object. (virtual constructor)
virtual ~VelocityProfile() {}
};
}
#endif

71
kdl_install/include/kdl/velocityprofile_dirac.hpp Normal file
View File

@@ -0,0 +1,71 @@
/***************************************************************************
tag: Peter Soetens Fri Feb 11 15:59:12 CET 2005 velocityprofile_dirac.h
velocityprofile_dirac.h - description
-------------------
begin : Fri February 11 2005
copyright : (C) 2005 Peter Soetens
email : peter.soetens@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
#ifndef MOTIONPROFILE_DIRAC_H
#define MOTIONPROFILE_DIRAC_H
#include "velocityprofile.hpp"
namespace KDL {
/**
* A Dirac VelocityProfile generates an infinite velocity
* so that the position jumps from A to B in in infinite short time.
* In practice, this means that the maximum values are ignored and
* for any t : Vel(t) == 0 and Acc(t) == 0.
* Further Pos( -0 ) = pos1 and Pos( +0 ) = pos2.
*
* However, if a duration is given, it will create an unbound
* rectangular velocity profile for that duration, otherwise,
* Duration() == 0;
* @ingroup Motion
*/
class VelocityProfile_Dirac : public VelocityProfile
{
double p1,p2,t;
public:
void SetProfile(double pos1,double pos2);
virtual void SetProfileDuration(double pos1,double pos2,double duration);
virtual double Duration() const;
virtual double Pos(double time) const;
virtual double Vel(double time) const;
virtual double Acc(double time) const;
virtual void Write(std::ostream& os) const;
virtual VelocityProfile* Clone() const {
VelocityProfile_Dirac* res = new VelocityProfile_Dirac();
res->SetProfileDuration( p1, p2, t );
return res;
}
virtual ~VelocityProfile_Dirac() {}
};
}
#endif

90
kdl_install/include/kdl/velocityprofile_rect.hpp Normal file
View File

@@ -0,0 +1,90 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 velocityprofile_rect.h
velocityprofile_rect.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: velocityprofile_rect.h,v 1.1.1.1.2.4 2003/07/24 13:26:15 psoetens Exp $
* $Name: $
****************************************************************************/
#ifndef MOTIONPROFILE_RECT_H
#define MOTIONPROFILE_RECT_H
#include "velocityprofile.hpp"
namespace KDL {
/**
* A rectangular VelocityProfile generates a constant velocity
* for moving from A to B.
* @ingroup Motion
*/
class VelocityProfile_Rectangular : public VelocityProfile
// Defines a rectangular velocityprofile.
// (i.e. constant velocity)
{
double d,p,v;
public:
double maxvel;
VelocityProfile_Rectangular(double _maxvel=0):
maxvel(_maxvel) {}
// constructs motion profile class with <maxvel> as parameter of the
// trajectory.
void SetMax( double _maxvel );
void SetProfile(double pos1,double pos2);
virtual void SetProfileDuration(
double pos1,double pos2,double duration);
virtual double Duration() const;
virtual double Pos(double time) const;
virtual double Vel(double time) const;
virtual double Acc(double time) const;
virtual void Write(std::ostream& os) const;
virtual VelocityProfile* Clone() const{
VelocityProfile_Rectangular* res = new VelocityProfile_Rectangular(maxvel);
res->SetProfileDuration( p, p+v*d, d );
return res;
}
// returns copy of current VelocityProfile object. (virtual constructor)
virtual ~VelocityProfile_Rectangular() {}
};
}
#endif

67
kdl_install/include/kdl/velocityprofile_spline.hpp Normal file
View File

@@ -0,0 +1,67 @@
#ifndef VELOCITYPROFILE_SPLINE_H
#define VELOCITYPROFILE_SPLINE_H
#include "velocityprofile.hpp"
namespace KDL
{
/**
* \brief A spline VelocityProfile trajectory interpolation.
* @ingroup Motion
*/
class VelocityProfile_Spline : public VelocityProfile
{
public:
VelocityProfile_Spline();
VelocityProfile_Spline(const VelocityProfile_Spline &p);
virtual ~VelocityProfile_Spline();
virtual void SetProfile(double pos1, double pos2);
/**
* Generate linear interpolation coefficients.
*
* @param pos1 begin position.
* @param pos2 end position.
* @param duration duration of the profile.
*/
virtual void SetProfileDuration(
double pos1, double pos2, double duration);
/**
* Generate cubic spline interpolation coefficients.
*
* @param pos1 begin position.
* @param vel1 begin velocity.
* @param pos2 end position.
* @param vel2 end velocity.
* @param duration duration of the profile.
*/
virtual void SetProfileDuration(
double pos1, double vel1, double pos2, double vel2, double duration);
/**
* Generate quintic spline interpolation coefficients.
*
* @param pos1 begin position.
* @param vel1 begin velocity.
* @param acc1 begin acceleration
* @param pos2 end position.
* @param vel2 end velocity.
* @param acc2 end acceleration.
* @param duration duration of the profile.
*/
virtual void SetProfileDuration(double pos1, double vel1, double acc1, double pos2, double vel2, double acc2, double duration);
virtual double Duration() const;
virtual double Pos(double time) const;
virtual double Vel(double time) const;
virtual double Acc(double time) const;
virtual void Write(std::ostream& os) const;
virtual VelocityProfile* Clone() const;
private:
double coeff_[6];
double duration_;
};
}
#endif // VELOCITYPROFILE_CUBICSPLINE_H

142
kdl_install/include/kdl/velocityprofile_trap.hpp Normal file
View File

@@ -0,0 +1,142 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 velocityprofile_trap.h
velocityprofile_trap.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: velocityprofile_trap.h,v 1.1.1.1.2.5 2003/07/24 13:26:15 psoetens Exp $
* $Name: $
****************************************************************************/
#ifndef KDL_MOTION_VELOCITYPROFILE_TRAP_H
#define KDL_MOTION_VELOCITYPROFILE_TRAP_H
#include "velocityprofile.hpp"
namespace KDL {
/**
* A Trapezoidal VelocityProfile implementation.
* @ingroup Motion
*/
class VelocityProfile_Trap : public VelocityProfile
{
// For "running" a motion profile :
double a1,a2,a3; // coef. from ^0 -> ^2 of first part
double b1,b2,b3; // of 2nd part
double c1,c2,c3; // of 3rd part
double duration;
double t1,t2;
// specification of the motion profile :
double maxvel;
double maxacc;
double startpos;
double endpos;
public:
VelocityProfile_Trap(double _maxvel=0,double _maxacc=0);
// constructs motion profile class with <maxvel> and <maxacc> as parameters of the
// trajectory.
virtual void SetProfile(double pos1,double pos2);
virtual void SetProfileDuration(
double pos1,double pos2,double newduration
);
/** Compute trapezoidal profile at a given fraction of max velocity
@param pos1 Position to start from
@param pos2 Position to end at
@param newvelocity Fraction of max velocity to use during the
non-ramp, flat-velocity part of the profile.
@param KDL::epsilon <= newvelocity <= 1.0 (forcibly clamped to
this range internally)
*/
virtual void SetProfileVelocity(
double pos1,double pos2,double newvelocity
);
virtual void SetMax(double _maxvel,double _maxacc);
virtual double Duration() const;
virtual double Pos(double time) const;
virtual double Vel(double time) const;
virtual double Acc(double time) const;
virtual void Write(std::ostream& os) const;
virtual VelocityProfile* Clone() const;
// returns copy of current VelocityProfile object. (virtual constructor)
virtual ~VelocityProfile_Trap();
};
/* Niet OK
class VelocityProfile_Trap : public VelocityProfile {
double maxvel;
double maxacc;
double _t1,_t2,_T,c1,c2,c3,c4,c5,c6,c7,c8,c9,c10;
void PrepTraj(double p1,double v1,double p2,double v2,
double acc,double vel,double t1,double t2,double T);
// Internal method. Sets the parameters <_t1>,..<c10> with the given
// arguments.
public:
VelocityProfile_Trap(double _maxvel,double _maxacc):
maxvel(_maxvel),maxacc(_maxacc) {}
// constructs motion profile class with max velocity <maxvel>,
// and max acceleration <maxacc> as parameter of the
// trajectory.
void SetProfile(double pos1,double pos2);
virtual void SetProfileDuration(double pos1,double pos2,double duration);
virtual double Duration() ;
virtual double Pos(double time);
virtual double Vel(double time);
};
*/
}
#endif

136
kdl_install/include/kdl/velocityprofile_traphalf.hpp Normal file
View File

@@ -0,0 +1,136 @@
/***************************************************************************
tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 velocityprofile_traphalf.h
velocityprofile_traphalf.h - description
-------------------
begin : Mon January 10 2005
copyright : (C) 2005 Erwin Aertbelien
email : erwin.aertbelien@mech.kuleuven.ac.be
***************************************************************************
* This library 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 2.1 of the License, or (at your option) any later version. *
* *
* This library 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 *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
/*****************************************************************************
* \author
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
*
* \version
* ORO_Geometry V0.2
*
* \par History
* - $log$
*
* \par Release
* $Id: velocityprofile_traphalf.h,v 1.1.1.1.2.4 2003/07/24 13:26:15 psoetens Exp $
* $Name: $
* \par Status
* Experimental
****************************************************************************/
#ifndef KDL_MOTION_VELOCITYPROFILE_TRAPHALF_H
#define KDL_MOTION_VELOCITYPROFILE_TRAPHALF_H
#include "velocityprofile.hpp"
namespace KDL {
/**
* A 'Half' Trapezoidal VelocityProfile. A constructor flag
* indicates if the calculated profile should be starting
* or ending.
* @ingroup Motion
*/
class VelocityProfile_TrapHalf : public VelocityProfile
{
// For "running" a motion profile :
double a1,a2,a3; // coef. from ^0 -> ^2 of first part
double b1,b2,b3; // of 2nd part
double c1,c2,c3; // of 3rd part
double duration;
double t1,t2;
double startpos;
double endpos;
// Persistent state :
double maxvel;
double maxacc;
bool starting;
void PlanProfile1(double v,double a);
void PlanProfile2(double v,double a);
public:
/**
* \param maxvel maximal velocity of the motion profile (positive)
* \param maxacc maximal acceleration of the motion profile (positive)
* \param starting this value is true when initial velocity is zero
* and ending velocity is maxvel, is false for the reverse
*/
VelocityProfile_TrapHalf(double _maxvel=0,double _maxacc=0,bool _starting=true);
void SetMax(double _maxvel,double _maxacc,bool _starting);
/**
* Plans a 'Half' Trapezoidal VelocityProfile between pos1 and pos2.
* If the distance is too short between pos1 and pos2,
* only the acceleration phase is set and the max velocity is not reached.
*
* \param pos1 Starting position
* \param pos2 Ending position
*
* Can throw a Error_MotionPlanning_Not_Feasible
*/
virtual void SetProfile(double pos1,double pos2);
/**
* Can be used to prolong the profile, there are two possible outcomes: in a first
* phase the acceleration is lowered as such that the end position and maximum velocity
* are reached at the given duration (newduration). In this case there is an acceleration part and a constant velocity part,
* when this reaches a minimum acceleration value at which the constant part disappears, the motion is stalled,
* in this case their is a non-motion part and an acceleration part.
*
*\param pos1 starting position
*\param pos2 ending position
*\param newduration the desired duration, if it is lower than the minimum duration, the minimum duration will be used instead of the given duration.
*/
virtual void SetProfileDuration(
double pos1,double pos2,double newduration
);
virtual double Duration() const;
virtual double Pos(double time) const;
virtual double Vel(double time) const;
virtual double Acc(double time) const;
virtual void Write(std::ostream& os) const;
virtual VelocityProfile* Clone() const;
virtual ~VelocityProfile_TrapHalf();
};
}
#endif