suzanne.soy/FreeCAD
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Upgraded KDL to latest version

Browse Source
This commit is contained in:
Yorik van Havre 2015-10-12 19:35:53 -03:00
parent 85994d17b8
commit 9f4e7c4085
104 changed files with 3812 additions and 922 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

151
src/Mod/Robot/App/kdl_cp/CMakeLists.txt
View File

@ -1,29 +1,146 @@
FILE( GLOB KDL_SRCS [^.]*.cpp )
FILE( GLOB_RECURSE KDL_SRCS [^.]*.cpp [^.]*.cxx)
FILE( GLOB KDL_HPPS [^.]*.hpp [^.]*.inl)
FILE( GLOB UTIL_SRCS utilities/[^.]*.cpp utilities/[^.]*.cxx)
FILE( GLOB UTIL_HPPS utilities/[^.]*.h utilities/[^.]*.hpp)
ADD_LIBRARY(orocos-kdl SHARED ${KDL_SRCS} ${UTIL_SRCS})
INCLUDE(CheckCXXSourceCompiles)
SET(CMAKE_REQUIRED_FLAGS)
CHECK_CXX_SOURCE_COMPILES("
#include <string>
#include <map>
#include <vector.hpp>
class TreeElement;
typedef std::map<std::string, TreeElement> SegmentMap;
class TreeElement
{
TreeElement(const std::string& name): number(0) {}
public:
int number;
SegmentMap::const_iterator parent;
std::vector<SegmentMap::const_iterator> children;
static TreeElement Root(std::string& name)
{
return TreeElement(name);
}
};
int main()
{
return 0;
}
"
HAVE_STL_CONTAINER_INCOMPLETE_TYPES)
if(HAVE_STL_CONTAINER_INCOMPLETE_TYPES)
SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT Off)
ELSE(HAVE_STL_CONTAINER_INCOMPLETE_TYPES)
SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT On)
ENDIF(HAVE_STL_CONTAINER_INCOMPLETE_TYPES)
SET(KDL_USE_NEW_TREE_INTERFACE ${KDL_USE_NEW_TREE_INTERFACE_DEFAULT} CACHE BOOL "Use the new KDL Tree interface")
#Sanity check, inform the user
IF(NOT HAVE_STL_CONTAINER_INCOMPLETE_TYPES AND NOT KDL_USE_NEW_TREE_INTERFACE)
MESSAGE(WARNING "You have chosen to use the current Tree Interface, but your platform doesn't support containers of "
"incomplete types, this configuration is likely invalid")
ENDIF()
#In Windows (Visual Studio) it is necessary to specify the postfix
#of the debug library name and no symbols are exported by kdl,
#so it is necessary to compile it as a static library
IF(MSVC)
SET(CMAKE_DEBUG_POSTFIX "d")
SET(LIB_TYPE STATIC)
ELSE(MSVC)
SET(LIB_TYPE SHARED)
ENDIF(MSVC)
CONFIGURE_FILE(config.h.in config.h @ONLY)
#### Settings for rpath
IF(${CMAKE_MINIMUM_REQUIRED_VERSION} VERSION_GREATER "2.8.12")
MESSAGE(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed.")
ENDIF()
IF(NOT (CMAKE_VERSION VERSION_LESS 2.8.12))
IF(NOT MSVC)
#add the option to disable RPATH
OPTION(OROCOSKDL_ENABLE_RPATH "Enable RPATH during installation" FALSE)
MARK_AS_ADVANCED(OROCOSKDL_ENABLE_RPATH)
ENDIF(NOT MSVC)
IF(OROCOSKDL_ENABLE_RPATH)
#Configure RPATH
SET(CMAKE_MACOSX_RPATH TRUE) #enable RPATH on OSX. This also suppress warnings on CMake >= 3.0
# when building, don't use the install RPATH already
# (but later on when installing)
SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
#build directory by default is built with RPATH
SET(CMAKE_SKIP_BUILD_RPATH FALSE)
#This is relative RPATH for libraries built in the same project
#I assume that the directory is
# - install_dir/something for binaries
# - install_dir/lib for libraries
LIST(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir)
IF("${isSystemDir}" STREQUAL "-1")
FILE(RELATIVE_PATH _rel_path "${CMAKE_INSTALL_PREFIX}/bin" "${CMAKE_INSTALL_PREFIX}/lib")
IF (${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
SET(CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}")
ELSE()
SET(CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}")
ENDIF()
ENDIF("${isSystemDir}" STREQUAL "-1")
# add the automatically determined parts of the RPATH
# which point to directories outside the build tree to the install RPATH
SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) #very important!
ENDIF()
ENDIF()
#####end RPATH
ADD_LIBRARY(orocos-kdl ${LIB_TYPE} ${KDL_SRCS} config.h)
SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES
SOVERSION "${KDL_VERSION_MAJOR}.${KDL_VERSION_MINOR}"
VERSION "${KDL_VERSION}"
COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS}")
COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS}"
PUBLIC_HEADER "${KDL_HPPS};${CMAKE_CURRENT_BINARY_DIR}/config.h"
)
INSTALL_TARGETS( /lib orocos-kdl)
#### Settings for rpath disabled (back-compatibility)
IF(${CMAKE_MINIMUM_REQUIRED_VERSION} VERSION_GREATER "2.8.12")
MESSAGE(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed.")
ENDIF()
IF(CMAKE_VERSION VERSION_LESS 2.8.12)
SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES
INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}")
ELSE()
IF(NOT OROCOSKDL_ENABLE_RPATH)
SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES
INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}")
ENDIF()
ENDIF()
#####end RPATH
# CMake 2.2:
INSTALL_FILES( /include/kdl FILES ${KDL_HPPS})
INSTALL_FILES( /include/kdl/utilities FILES ${UTIL_HPPS})
# Needed so that the generated config.h can be used
INCLUDE_DIRECTORIES(${CMAKE_CURRENT_BINARY_DIR})
TARGET_LINK_LIBRARIES(orocos-kdl ${Boost_LIBRARIES})
INSTALL(TARGETS orocos-kdl
EXPORT OrocosKDLTargets
ARCHIVE DESTINATION lib${LIB_SUFFIX}
LIBRARY DESTINATION lib${LIB_SUFFIX}
PUBLIC_HEADER DESTINATION include/kdl
)
INSTALL(FILES ${UTIL_HPPS} DESTINATION include/kdl/utilities)
# Orocos convention:
CONFIGURE_FILE( kdl.pc.in src/orocos-kdl.pc @ONLY)
INSTALL_FILES( /lib/pkgconfig FILES orocos-kdl.pc)
CONFIGURE_FILE( kdl.pc.in ${CMAKE_CURRENT_BINARY_DIR}/orocos-kdl.pc @ONLY)
CONFIGURE_FILE( kdl.pc.in ${CMAKE_CURRENT_BINARY_DIR}/orocos_kdl.pc @ONLY)
IF( OROCOS_PLUGIN )
ADD_SUBDIRECTORY( bindings/rtt )
ENDIF( OROCOS_PLUGIN )
IF( PYTHON_BINDINGS )
ADD_SUBDIRECTORY( bindings/python )
ENDIF( PYTHON_BINDINGS )
INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/orocos-kdl.pc DESTINATION lib${LIB_SUFFIX}/pkgconfig)
INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/orocos_kdl.pc DESTINATION lib${LIB_SUFFIX}/pkgconfig)

14
src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.cpp
View File

@ -27,10 +27,10 @@ using namespace Eigen;
namespace KDL{
ArticulatedBodyInertia::ArticulatedBodyInertia(const RigidBodyInertia& rbi)
ArticulatedBodyInertia::ArticulatedBodyInertia(const RigidBodyInertia& rbi)
{
this->M.part<SelfAdjoint>()=Matrix3d::Identity()*rbi.m;
this->I.part<SelfAdjoint>()=Map<Matrix3d>(rbi.I.data);
this->M=Matrix3d::Identity()*rbi.m;
this->I=Map<const Matrix3d>(rbi.I.data);
this->H << 0,-rbi.h[2],rbi.h[1],
rbi.h[2],0,-rbi.h[0],
-rbi.h[1],rbi.h[0],0;
@ -41,10 +41,10 @@ namespace KDL{
*this = RigidBodyInertia(m,c,Ic);
}
ArticulatedBodyInertia::ArticulatedBodyInertia(const Eigen::Matrix3d& M, const Eigen::Matrix3d& H, const Eigen::Matrix3d& I)
ArticulatedBodyInertia::ArticulatedBodyInertia(const Matrix3d& M, const Matrix3d& H, const Matrix3d& I)
{
this->M.part<SelfAdjoint>()=M;
this->I.part<SelfAdjoint>()=I;
this->M=M;
this->I=I;
this->H=H;
}
@ -90,7 +90,7 @@ namespace KDL{
}
ArticulatedBodyInertia operator*(const Rotation& M,const ArticulatedBodyInertia& I){
Map<Matrix3d> E(M.data);
Map<const Matrix3d> E(M.data);
return ArticulatedBodyInertia(E.transpose()*I.M*E,E.transpose()*I.H*E,E.transpose()*I.I*E);
}

54
src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.hpp
View File

@ -57,7 +57,7 @@ namespace KDL {
* 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.
*/
ArticulatedBodyInertia(double m, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero());
explicit ArticulatedBodyInertia(double m, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero());
/**
* Creates an inertia with zero mass, and zero RotationalInertia
@ -69,34 +69,13 @@ namespace KDL {
~ArticulatedBodyInertia(){};
/**
* Scalar product: I_new = double * I_old
*/
friend 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
*/
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);
/**
* calculate spatial momentum: h = I*v
* make sure that the twist v and the inertia are expressed in the same reference frame/point
*/
friend 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.
*/
friend 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
*/
friend ArticulatedBodyInertia operator*(const Rotation& R,const ArticulatedBodyInertia& I);
/**
@ -111,7 +90,36 @@ namespace KDL {
Eigen::Matrix3d H;
Eigen::Matrix3d I;
};
}//namespace
/**
* 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

14
src/Mod/Robot/App/kdl_cp/chain.cpp
View File

@ -25,14 +25,16 @@ namespace KDL {
using namespace std;
Chain::Chain():
nrOfJoints(0),
nrOfSegments(0),
segments(0)
nrOfJoints(0),
nrOfSegments(0),
segments(0)
{
}
Chain::Chain(const Chain& in):nrOfJoints(0),
nrOfSegments(0)
Chain::Chain(const Chain& in):
nrOfJoints(0),
nrOfSegments(0),
segments(0)
{
for(unsigned int i=0;i<in.getNrOfSegments();i++)
this->addSegment(in.getSegment(i));
@ -43,7 +45,7 @@ namespace KDL {
nrOfJoints=0;
nrOfSegments=0;
segments.resize(0);
for(int i=0;i<arg.nrOfSegments;i++)
for(unsigned int i=0;i<arg.nrOfSegments;i++)
addSegment(arg.getSegment(i));
return *this;

4
src/Mod/Robot/App/kdl_cp/chain.hpp
View File

@ -34,8 +34,8 @@ namespace KDL {
*/
class Chain {
private:
int nrOfJoints;
int nrOfSegments;
unsigned int nrOfJoints;
unsigned int nrOfSegments;
public:
std::vector<Segment> segments;
/**

134
src/Mod/Robot/App/kdl_cp/chaindynparam.cpp
View File

@ -26,83 +26,84 @@
namespace KDL {
ChainDynParam::ChainDynParam(const Chain& _chain, Vector _grav):
chain(_chain),
nj(chain.getNrOfJoints()),
ns(chain.getNrOfSegments()),
grav(_grav),
jntarraynull(nj),
chainidsolver_coriolis( chain, Vector::Zero()),
chainidsolver_gravity( chain, grav),
wrenchnull(ns,Wrench::Zero()),
X(ns),
S(ns),
Ic(ns)
chain(_chain),
nr(0),
nj(chain.getNrOfJoints()),
ns(chain.getNrOfSegments()),
grav(_grav),
jntarraynull(nj),
chainidsolver_coriolis( chain, Vector::Zero()),
chainidsolver_gravity( chain, grav),
wrenchnull(ns,Wrench::Zero()),
X(ns),
S(ns),
Ic(ns)
{
ag=-Twist(grav,Vector::Zero());
ag=-Twist(grav,Vector::Zero());
}
//calculate inertia matrix H
int ChainDynParam::JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H)
{
//Check sizes when in debug mode
//Check sizes when in debug mode
if(q.rows()!=nj || H.rows()!=nj || H.columns()!=nj )
return -1;
unsigned int k=0;
double q_;
//Sweep from root to leaf
double q_;
//Sweep from root to leaf
for(unsigned int i=0;i<ns;i++)
{
//Collect RigidBodyInertia
{
//Collect RigidBodyInertia
Ic[i]=chain.getSegment(i).getInertia();
if(chain.getSegment(i).getJoint().getType()!=Joint::None)
{
q_=q(k);
k++;
}
else
{
q_=0.0;
}
X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the frame for transformations from the parent to the current coord frame
S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
{
q_=q(k);
k++;
}
else
{
q_=0.0;
}
X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the frame for transformations from the parent to the current coord frame
S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
}
//Sweep from leaf to root
//Sweep from leaf to root
int j,l;
k=nj-1; //reset k
k=nj-1; //reset k
for(int i=ns-1;i>=0;i--)
{
{
if(i!=0)
{
//assumption that previous segment is parent
Ic[i-1]=Ic[i-1]+X[i]*Ic[i];
}
if(i!=0)
{
//assumption that previous segment is parent
Ic[i-1]=Ic[i-1]+X[i]*Ic[i];
}
F=Ic[i]*S[i];
if(chain.getSegment(i).getJoint().getType()!=Joint::None)
{
H(k,k)=dot(S[i],F);
j=k; //countervariable for the joints
l=i; //countervariable for the segments
while(l!=0) //go from leaf to root starting at i
{
//assumption that previous segment is parent
F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l]
l--; //go down a segment
if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint
{
j--;
H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment
H(j,k)=H(k,j);
}
}
k--; //this if-loop should be repeated nj times (k=nj-1 to k=0)
}
F=Ic[i]*S[i];
if(chain.getSegment(i).getJoint().getType()!=Joint::None)
{
H(k,k)=dot(S[i],F);
j=k; //countervariable for the joints
l=i; //countervariable for the segments
while(l!=0) //go from leaf to root starting at i
{
//assumption that previous segment is parent
F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l]
l--; //go down a segment
if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint
{
j--;
H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment
H(j,k)=H(k,j);
}
}
k--; //this if-loop should be repeated nj times (k=nj-1 to k=0)
}
} // for
return 0;
}
return 0;
}
//calculate coriolis matrix C
@ -111,11 +112,11 @@ namespace KDL {
//make a null matrix with the size of q_dotdot and a null wrench
SetToZero(jntarraynull);
//the calculation of coriolis matrix C
return chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis);
chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis);
return 0;
}
//calculate gravity matrix G
@ -123,10 +124,11 @@ namespace KDL {
{
//make a null matrix with the size of q_dotdot and a null wrench
SetToZero(jntarraynull);
//the calculation of coriolis matrix C
return chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity);
chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity);
return 0;
}
ChainDynParam::~ChainDynParam()

4
src/Mod/Robot/App/kdl_cp/chaindynparam.hpp
View File

@ -47,7 +47,7 @@ namespace KDL {
{
public:
ChainDynParam(const Chain& chain, Vector _grav);
~ChainDynParam();
virtual ~ChainDynParam();
virtual int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis);
virtual int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H);
@ -57,7 +57,7 @@ namespace KDL {
const Chain chain;
int nr;
unsigned int nj;
unsigned int ns;
unsigned int ns;
Vector grav;
Vector vectornull;
JntArray jntarraynull;

12
src/Mod/Robot/App/kdl_cp/chainfksolver.hpp
View File

@ -28,6 +28,7 @@
#include "jntarray.hpp"
#include "jntarrayvel.hpp"
#include "jntarrayacc.hpp"
#include "solveri.hpp"
namespace KDL {
@ -39,7 +40,7 @@ namespace KDL {
*/
//Forward definition
class ChainFkSolverPos {
class ChainFkSolverPos : public KDL::SolverI {
public:
/**
* Calculate forward position kinematics for a KDL::Chain,
@ -47,7 +48,6 @@ namespace KDL {
*
* @param q_in input joint coordinates
* @param p_out reference to output cartesian pose
* @param segmentNr default to -1
*
* @return if < 0 something went wrong
*/
@ -61,7 +61,7 @@ namespace KDL {
*
* @ingroup KinematicFamily
*/
class ChainFkSolverVel {
class ChainFkSolverVel : public KDL::SolverI {
public:
/**
* Calculate forward position and velocity kinematics, from
@ -69,7 +69,6 @@ namespace KDL {
*
* @param q_in input joint coordinates (position and velocity)
* @param out output cartesian coordinates (position and velocity)
* @param segmentNr default to -1
*
* @return if < 0 something went wrong
*/
@ -85,7 +84,7 @@ namespace KDL {
* @ingroup KinematicFamily
*/
class ChainFkSolverAcc {
class ChainFkSolverAcc : public KDL::SolverI {
public:
/**
* Calculate forward position, velocity and accelaration
@ -93,9 +92,8 @@ namespace KDL {
*
* @param q_in input joint coordinates (position, velocity and
* acceleration
* @param out output cartesian coordinates (position, velocity
@param out output cartesian coordinates (position, velocity
* and acceleration
* @param segmentNr default to -1
*
* @return if < 0 something went wrong
*/

14
src/Mod/Robot/App/kdl_cp/chainfksolverpos_recursive.cpp
View File

@ -30,20 +30,22 @@ namespace KDL {
{
}
int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr)
{
if(segmentNr<0)
segmentNr=chain.getNrOfSegments();
int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int seg_nr) {
unsigned int segmentNr;
if(seg_nr<0)
segmentNr=chain.getNrOfSegments();
else
segmentNr = seg_nr;
p_out = Frame::Identity();
if(q_in.rows()!=chain.getNrOfJoints())
return -1;
else if(segmentNr>static_cast<int>(chain.getNrOfSegments()))
else if(segmentNr>chain.getNrOfSegments())
return -1;
else{
int j=0;
for(int i=0;i<segmentNr;i++){
for(unsigned int i=0;i<segmentNr;i++){
if(chain.getSegment(i).getJoint().getType()!=Joint::None){
p_out = p_out*chain.getSegment(i).pose(q_in(j));
j++;

14
src/Mod/Robot/App/kdl_cp/chainfksolvervel_recursive.cpp
View File

@ -33,21 +33,23 @@ namespace KDL
{
}
int ChainFkSolverVel_recursive::JntToCart(const JntArrayVel& in,FrameVel& out,int segmentNr)
int ChainFkSolverVel_recursive::JntToCart(const JntArrayVel& in,FrameVel& out,int seg_nr)
{
if(segmentNr<0)
segmentNr=chain.getNrOfSegments();
unsigned int segmentNr;
if(seg_nr<0)
segmentNr=chain.getNrOfSegments();
else
segmentNr = seg_nr;
out=FrameVel::Identity();
if(!(in.q.rows()==chain.getNrOfJoints()&&in.qdot.rows()==chain.getNrOfJoints()))
return -1;
else if(segmentNr>static_cast<int>(chain.getNrOfSegments()))
else if(segmentNr>chain.getNrOfSegments())
return -1;
else{
int j=0;
for (int i=0;i<segmentNr;i++) {
for (unsigned int i=0;i<segmentNr;i++) {
//Calculate new Frame_base_ee
if(chain.getSegment(i).getJoint().getType()!=Joint::None){
out=out*FrameVel(chain.getSegment(i).pose(in.q(j)),

10
src/Mod/Robot/App/kdl_cp/chainidsolver.hpp
View File

@ -25,6 +25,7 @@
#include "chain.hpp"
#include "frames.hpp"
#include "jntarray.hpp"
#include "solveri.hpp"
namespace KDL
{
@ -36,19 +37,18 @@ namespace KDL
* dynamics solver for a KDL::Chain.
*
*/
class ChainIdSolver
class ChainIdSolver : public KDL::SolverI
{
public:
/**
* Calculate inverse dynamics, from joint positions,
* velocity, acceleration, external forces
* 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 external forces
* @param torques output joint torques
*
* @param torque output joint torques
*
* @return if < 0 something went wrong
*/

14
src/Mod/Robot/App/kdl_cp/chainidsolver_recursive_newton_euler.cpp
View File

@ -23,7 +23,7 @@
#include "frames_io.hpp"
namespace KDL{
ChainIdSolver_RNE::ChainIdSolver_RNE(const Chain& chain_,Vector grav):
chain(chain_),nj(chain.getNrOfJoints()),ns(chain.getNrOfSegments()),
X(ns),S(ns),v(ns),a(ns),f(ns)
@ -35,7 +35,7 @@ namespace KDL{
{
//Check sizes when in debug mode
if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
return -1;
return (error = -1);
unsigned int j=0;
//Sweep from root to leaf
@ -48,10 +48,10 @@ namespace KDL{
j++;
}else
q_=qdot_=qdotdot_=0.0;
//Calculate segment properties: X,S,vj,cj
X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the
//frame for transformations from
X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the
//frame for transformations from
//the parent to the current coord frame
//Transform velocity and unit velocity to segment frame
Twist vj=X[i].M.Inverse(chain.getSegment(i).twist(q_,qdot_));
@ -69,7 +69,7 @@ namespace KDL{
//Collect RigidBodyInertia and external forces
RigidBodyInertia Ii=chain.getSegment(i).getInertia();
f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];
//std::cout << "a[i]=" << a[i] << "\n f[i]=" << f[i] << "\n S[i]" << S[i] << std::endl;
//std::cout << "a[i]=" << a[i] << "\n f[i]=" << f[i] << "\n S[i]" << S[i] << std::endl;
}
//Sweep from leaf to root
j=nj-1;
@ -79,6 +79,6 @@ namespace KDL{
if(i!=0)
f[i-1]=f[i-1]+X[i]*f[i];
}
return 0;
return (error = E_NOERROR);
}
}//namespace

472
src/Mod/Robot/App/kdl_cp/chainidsolver_vereshchagin.cpp Normal file
View File

@ -0,0 +1,472 @@
// Copyright (C) 2009, 2011
// Version: 1.0
// Author: Ruben Smits, Herman Bruyninckx, Azamat Shakhimardanov
// Maintainer: Ruben Smits, Azamat Shakhimardanov
// 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
#include "chainidsolver_vereshchagin.hpp"
#include "frames_io.hpp"
#include "utilities/svd_eigen_HH.hpp"
namespace KDL
{
using namespace Eigen;
ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin(const Chain& chain_, Twist root_acc, unsigned int _nc) :
chain(chain_), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), nc(_nc),
results(ns + 1, segment_info(nc))
{
acc_root = root_acc;
//Provide the necessary memory for computing the inverse of M0
nu_sum.resize(nc);
M_0_inverse.resize(nc, nc);
Um = MatrixXd::Identity(nc, nc);
Vm = MatrixXd::Identity(nc, nc);
Sm = VectorXd::Ones(nc);
tmpm = VectorXd::Ones(nc);
}
int ChainIdSolver_Vereshchagin::CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques)
{
//Check sizes always
if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || torques.rows() != nj || f_ext.size() != ns)
return -1;
if (alfa.columns() != nc || beta.rows() != nc)
return -2;
//do an upward recursion for position and velocities
this->initial_upwards_sweep(q, q_dot, q_dotdot, f_ext);
//do an inward recursion for inertia, forces and constraints
this->downwards_sweep(alfa, torques);
//Solve for the constraint forces
this->constraint_calculation(beta);
//do an upward recursion to propagate the result
this->final_upwards_sweep(q_dotdot, torques);
return 0;
}
void ChainIdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const JntArray &qdot, const JntArray &qdotdot, const Wrenches& f_ext)
{
//if (q.rows() != nj || qdot.rows() != nj || qdotdot.rows() != nj || f_ext.size() != ns)
// return -1;
unsigned int j = 0;
F_total = Frame::Identity();
for (unsigned int i = 0; i < ns; i++)
{
//Express everything in the segments reference frame (body coordinates)
//which is at the segments tip, i.e. where the next joint is attached.
//Calculate segment properties: X,S,vj,cj
const Segment& segment = chain.getSegment(i);
segment_info& s = results[i + 1];
//The pose between the joint root and the segment tip (tip expressed in joint root coordinates)
s.F = segment.pose(q(j)); //X pose of each link in link coord system
F_total = F_total * s.F; //X pose of the each link in root coord system
s.F_base = F_total; //X pose of the each link in root coord system for getter functions
//The velocity due to the joint motion of the segment expressed in the segments reference frame (tip)
Twist vj = s.F.M.Inverse(segment.twist(q(j), qdot(j))); //XDot of each link
//Twist aj = s.F.M.Inverse(segment.twist(q(j), qdotdot(j))); //XDotDot of each link
//The unit velocity due to the joint motion of the segment expressed in the segments reference frame (tip)
s.Z = s.F.M.Inverse(segment.twist(q(j), 1.0));
//Put Z in the joint root reference frame:
s.Z = s.F * s.Z;
//The total velocity of the segment expressed in the the segments reference frame (tip)
if (i != 0)
{
s.v = s.F.Inverse(results[i].v) + vj; // recursive velocity of each link in segment frame
//s.A=s.F.Inverse(results[i].A)+aj;
s.A = s.F.M.Inverse(results[i].A);
}
else
{
s.v = vj;
s.A = s.F.M.Inverse(acc_root);
}
//c[i] = cj + v[i]xvj (remark: cj=0, since our S is not time dependent in local coordinates)
//The velocity product acceleration
//std::cout << i << " Initial upward" << s.v << std::endl;
s.C = s.v*vj; //This is a cross product: cartesian space BIAS acceleration in local link coord.
//Put C in the joint root reference frame
s.C = s.F * s.C; //+F_total.M.Inverse(acc_root));
//The rigid body inertia of the segment, expressed in the segments reference frame (tip)
s.H = segment.getInertia();
//wrench of the rigid body bias forces and the external forces on the segment (in body coordinates, tip)
//external forces are taken into account through s.U.
Wrench FextLocal = F_total.M.Inverse() * f_ext[i];
s.U = s.v * (s.H * s.v) - FextLocal; //f_ext[i];
if (segment.getJoint().getType() != Joint::None)
j++;
}
}
void ChainIdSolver_Vereshchagin::downwards_sweep(const Jacobian& alfa, const JntArray &torques)
{
int j = nj - 1;
for (int i = ns; i >= 0; i--)
{
//Get a handle for the segment we are working on.
segment_info& s = results[i];
//For segment N,
//tilde is in the segment refframe (tip, not joint root)
//without tilde is at the joint root (the childs tip!!!)
//P_tilde is the articulated body inertia
//R_tilde is the sum of external and coriolis/centrifugal forces
//M is the (unit) acceleration energy already generated at link i
//G is the (unit) magnitude of the constraint forces at link i
//E are the (unit) constraint forces due to the constraints
if (i == (int)ns)
{
s.P_tilde = s.H;
s.R_tilde = s.U;
s.M.setZero();
s.G.setZero();
//changeBase(alfa_N,F_total.M.Inverse(),alfa_N2);
for (unsigned int r = 0; r < 3; r++)
for (unsigned int c = 0; c < nc; c++)
{
//copy alfa constrain force matrix in E~
s.E_tilde(r, c) = alfa(r + 3, c);
s.E_tilde(r + 3, c) = alfa(r, c);
}
//Change the reference frame of alfa to the segmentN tip frame
//F_Total holds end effector frame, if done per segment bases then constraints could be extended to all segments
Rotation base_to_end = F_total.M.Inverse();
for (unsigned int c = 0; c < nc; c++)
{
Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)),
Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c)));
col = base_to_end*col;
s.E_tilde.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data);
}
}
else
{
//For all others:
//Everything should expressed in the body coordinates of segment i
segment_info& child = results[i + 1];
//Copy PZ into a vector so we can do matrix manipulations, put torques above forces
Vector6d vPZ;
vPZ << Vector3d::Map(child.PZ.torque.data), Vector3d::Map(child.PZ.force.data);
Matrix6d PZDPZt;
PZDPZt.noalias() = vPZ * vPZ.transpose();
PZDPZt /= child.D;
//equation a) (see Vereshchagin89) PZDPZt=[I,H;H',M]
//Azamat:articulated body inertia as in Featherstone (7.19)
s.P_tilde = s.H + child.P - ArticulatedBodyInertia(PZDPZt.bottomRightCorner<3,3>(), PZDPZt.topRightCorner<3,3>(), PZDPZt.topLeftCorner<3,3>());
//equation b) (see Vereshchagin89)
//Azamat: bias force as in Featherstone (7.20)
s.R_tilde = s.U + child.R + child.PC + (child.PZ / child.D) * child.u;
//equation c) (see Vereshchagin89)
s.E_tilde = child.E;
//Azamat: equation (c) right side term
s.E_tilde.noalias() -= (vPZ * child.EZ.transpose()) / child.D;
//equation d) (see Vereshchagin89)
s.M = child.M;
//Azamat: equation (d) right side term
s.M.noalias() -= (child.EZ * child.EZ.transpose()) / child.D;
//equation e) (see Vereshchagin89)
s.G = child.G;
Twist CiZDu = child.C + (child.Z / child.D) * child.u;
Vector6d vCiZDu;
vCiZDu << Vector3d::Map(CiZDu.rot.data), Vector3d::Map(CiZDu.vel.data);
s.G.noalias() += child.E.transpose() * vCiZDu;
}
if (i != 0)
{
//Transform all results to joint root coordinates of segment i (== body coordinates segment i-1)
//equation a)
s.P = s.F * s.P_tilde;
//equation b)
s.R = s.F * s.R_tilde;
//equation c), in matrix: torques above forces, so switch and switch back
for (unsigned int c = 0; c < nc; c++)
{
Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)),
Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c)));
col = s.F*col;
s.E.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data);
}
//needed for next recursion
s.PZ = s.P * s.Z;
s.D = dot(s.Z, s.PZ);
s.PC = s.P * s.C;
//u=(Q-Z(R+PC)=sum of external forces along the joint axes,
//R are the forces comming from the children,
//Q is taken zero (do we need to take the previous calculated torques?
//projection of coriolis and centrepital forces into joint subspace (0 0 Z)
s.totalBias = -dot(s.Z, s.R + s.PC);
s.u = torques(j) + s.totalBias;
//Matrix form of Z, put rotations above translations
Vector6d vZ;
vZ << Vector3d::Map(s.Z.rot.data), Vector3d::Map(s.Z.vel.data);
s.EZ.noalias() = s.E.transpose() * vZ;
if (chain.getSegment(i - 1).getJoint().getType() != Joint::None)
j--;
}
}
}
void ChainIdSolver_Vereshchagin::constraint_calculation(const JntArray& beta)
{
//equation f) nu = M_0_inverse*(beta_N - E0_tilde`*acc0 - G0)
//M_0_inverse, always nc*nc symmetric matrix
//std::cout<<"M0: "<<results[0].M<<std::endl;
//results[0].M-=MatrixXd::Identity(nc,nc);
//std::cout<<"augmented M0: "<<results[0].M<<std::endl;
//ToDo: Need to check ill conditions
//M_0_inverse=results[0].M.inverse();
svd_eigen_HH(results[0].M, Um, Sm, Vm, tmpm);
//truncated svd, what would sdls, dls physically mean?
for (unsigned int i = 0; i < nc; i++)
if (Sm(i) < 1e-14)
Sm(i) = 0.0;
else
Sm(i) = 1 / Sm(i);
results[0].M.noalias() = Vm * Sm.asDiagonal();
M_0_inverse.noalias() = results[0].M * Um.transpose();
//results[0].M.ldlt().solve(MatrixXd::Identity(nc,nc),&M_0_inverse);
//results[0].M.computeInverse(&M_0_inverse);
Vector6d acc;
acc << Vector3d::Map(acc_root.rot.data), Vector3d::Map(acc_root.vel.data);
nu_sum.noalias() = -(results[0].E_tilde.transpose() * acc);
//nu_sum.setZero();
nu_sum += beta.data;
nu_sum -= results[0].G;
//equation f) nu = M_0_inverse*(beta_N - E0_tilde`*acc0 - G0)
nu.noalias() = M_0_inverse * nu_sum;
}
void ChainIdSolver_Vereshchagin::final_upwards_sweep(JntArray &q_dotdot, JntArray &torques)
{
unsigned int j = 0;
for (unsigned int i = 1; i <= ns; i++)
{
segment_info& s = results[i];
//Calculation of joint and segment accelerations
//equation g) qdotdot[i] = D^-1*(Q - Z'(R + P(C + acc[i-1]) + E*nu))
// = D^-1(u - Z'(P*acc[i-1] + E*nu)
Twist a_g;
Twist a_p;
if (i == 1)
{
a_p = acc_root;
}
else
{
a_p = results[i - 1].acc;
}
//The contribution of the constraint forces at segment i
Vector6d tmp = s.E*nu;
Wrench constraint_force = Wrench(Vector(tmp(3), tmp(4), tmp(5)),
Vector(tmp(0), tmp(1), tmp(2)));
//acceleration components are also computed
//Contribution of the acceleration of the parent (i-1)
Wrench parent_force = s.P*a_p;
double parent_forceProjection = -dot(s.Z, parent_force);
double parentAccComp = parent_forceProjection / s.D;
//The constraint force and acceleration force projected on the joint axes -> axis torque/force
double constraint_torque = -dot(s.Z, constraint_force);
//The result should be the torque at this joint
torques(j) = constraint_torque;
//s.constAccComp = torques(j) / s.D;
s.constAccComp = constraint_torque / s.D;
s.nullspaceAccComp = s.u / s.D;
//total joint space acceleration resulting from accelerations of parent joints, constraint forces and
// nullspace forces.
q_dotdot(j) = (s.nullspaceAccComp + parentAccComp + s.constAccComp);
s.acc = s.F.Inverse(a_p + s.Z * q_dotdot(j) + s.C);//returns acceleration in link distal tip coordinates. For use needs to be transformed
if (chain.getSegment(i - 1).getJoint().getType() != Joint::None)
j++;
}
}
/*
void ChainIdSolver_Vereshchagin::getLinkCartesianPose(Frames& x_base)
{
for (int i = 0; i < ns; i++)
{
x_base[i] = results[i + 1].F_base;
}
return;
}
void ChainIdSolver_Vereshchagin::getLinkCartesianVelocity(Twists& xDot_base)
{
for (int i = 0; i < ns; i++)
{
xDot_base[i] = results[i + 1].F_base.M * results[i + 1].v;
}
return;
}
void ChainIdSolver_Vereshchagin::getLinkCartesianAcceleration(Twists& xDotDot_base)
{
for (int i = 0; i < ns; i++)
{
xDotDot_base[i] = results[i + 1].F_base.M * results[i + 1].acc;
//std::cout << "XDotDot_base[i] " << xDotDot_base[i] << std::endl;
}
return;
}
void ChainIdSolver_Vereshchagin::getLinkPose(Frames& x_local)
{
for (int i = 0; i < ns; i++)
{
x_local[i] = results[i + 1].F;
}
return;
}
void ChainIdSolver_Vereshchagin::getLinkVelocity(Twists& xDot_local)
{
for (int i = 0; i < ns; i++)
{
xDot_local[i] = results[i + 1].v;
}
return;
}
void ChainIdSolver_Vereshchagin::getLinkAcceleration(Twists& xDotdot_local)
{
for (int i = 0; i < ns; i++)
{
xDotdot_local[i] = results[i + 1].acc;
}
return;
}
void ChainIdSolver_Vereshchagin::getJointBiasAcceleration(JntArray& bias_q_dotdot)
{
for (int i = 0; i < ns; i++)
{
//this is only force
double tmp = results[i + 1].totalBias;
//this is accelleration
bias_q_dotdot(i) = tmp / results[i + 1].D;
//s.totalBias = - dot(s.Z, s.R + s.PC);
//std::cout << "totalBiasAccComponent" << i << ": " << bias_q_dotdot(i) << std::endl;
//bias_q_dotdot(i) = s.totalBias/s.D
}
return;
}
void ChainIdSolver_Vereshchagin::getJointConstraintAcceleration(JntArray& constraint_q_dotdot)
{
for (int i = 0; i < ns; i++)
{
constraint_q_dotdot(i) = results[i + 1].constAccComp;
//double tmp = results[i + 1].u;
//s.u = torques(j) + s.totalBias;
// std::cout << "s.constraintAccComp" << i << ": " << results[i+1].constAccComp << std::endl;
//nullspace_q_dotdot(i) = s.u/s.D
}
return;
}
//Check the name it does not seem to be appropriate
void ChainIdSolver_Vereshchagin::getJointNullSpaceAcceleration(JntArray& nullspace_q_dotdot)
{
for (int i = 0; i < ns; i++)
{
nullspace_q_dotdot(i) = results[i + 1].nullspaceAccComp;
//double tmp = results[i + 1].u;
//s.u = torques(j) + s.totalBias;
//std::cout << "s.nullSpaceAccComp" << i << ": " << results[i+1].nullspaceAccComp << std::endl;
//nullspace_q_dotdot(i) = s.u/s.D
}
return;
}
//This is not only a bias force energy but also includes generalized forces
//change type of parameter G
//this method should retur array of G's
void ChainIdSolver_Vereshchagin::getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G)
{
for (int i = 0; i < ns; i++)
{
G = results[i + 1].G;
//double tmp = results[i + 1].u;
//s.u = torques(j) + s.totalBias;
//std::cout << "s.G " << i << ": " << results[i+1].G << std::endl;
//nullspace_q_dotdot(i) = s.u/s.D
}
return;
}
//this method should retur array of R's
void ChainIdSolver_Vereshchagin::getLinkBiasForceMatrix(Wrenches& R_tilde)
{
for (int i = 0; i < ns; i++)
{
R_tilde[i] = results[i + 1].R_tilde;
//Azamat: bias force as in Featherstone (7.20)
//s.R_tilde = s.U + child.R + child.PC + child.PZ / child.D * child.u;
std::cout << "s.R_tilde " << i << ": " << results[i + 1].R_tilde << std::endl;
}
return;
}
*/
}//namespace

187
src/Mod/Robot/App/kdl_cp/chainidsolver_vereshchagin.hpp Normal file
View File

@ -0,0 +1,187 @@
// Copyright (C) 2009, 2011
// Version: 1.0
// Author: Ruben Smits, Herman Bruyninckx, Azamat Shakhimardanov
// Maintainer: Ruben Smits, Azamat Shakhimardanov
// 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 "chainidsolver.hpp"
#include "frames.hpp"
#include "articulatedbodyinertia.hpp"
namespace KDL
{
/**
* \brief Dynamics calculations by constraints based on Vereshchagin 1989.
* for a chain. This class creates instance of hybrid dynamics solver.
* The solver calculates total joint space accelerations in a chain when a constraint force(s) is applied
* to the chain's end-effector (task space/cartesian space).
*/
class ChainIdSolver_Vereshchagin
{
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 inverse dynamics for, an internal copy will be made.
* \param root_acc The acceleration vector of the root to use during the calculation.(most likely contains gravity)
*
*/
ChainIdSolver_Vereshchagin(const Chain& chain, Twist root_acc, unsigned int nc);
~ChainIdSolver_Vereshchagin()
{
};
/**
* This method calculates joint space constraint torques and total joint space acceleration.
* It returns 0 when it succeeds, otherwise -1 or -2 for unmatching matrix and array sizes.
* Input parameters;
* \param q The current joint positions
* \param q_dot The current joint velocities
* \param f_ext The external forces (no gravity, it is given in root acceleration) on the segments.
* Output parameters:
* \param q_dotdot The joint accelerations
* \param torques the resulting constraint torques for the joints
*/
int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques);
/*
//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 postions 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& 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 &torques);
private:
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;
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> results;
};
}
#endif

7
src/Mod/Robot/App/kdl_cp/chainiksolver.hpp
View File

@ -29,6 +29,7 @@
#include "jntarray.hpp"
#include "jntarrayvel.hpp"
#include "jntarrayacc.hpp"
#include "solveri.hpp"
namespace KDL {
@ -38,7 +39,7 @@ namespace KDL {
*
* @ingroup KinematicFamily
*/
class ChainIkSolverPos {
class ChainIkSolverPos : public KDL::SolverI {
public:
/**
* Calculate inverse position kinematics, from cartesian
@ -61,7 +62,7 @@ namespace KDL {
*
* @ingroup KinematicFamily
*/
class ChainIkSolverVel {
class ChainIkSolverVel : public KDL::SolverI {
public:
/**
* Calculate inverse velocity kinematics, from joint positions
@ -97,7 +98,7 @@ namespace KDL {
* @ingroup KinematicFamily
*/
class ChainIkSolverAcc {
class ChainIkSolverAcc : public KDL::SolverI {
public:
/**
* Calculate inverse acceleration kinematics from joint

286
src/Mod/Robot/App/kdl_cp/chainiksolverpos_lma.cpp Normal file
View File

@ -0,0 +1,286 @@
/**
\file chainiksolverpos_lma.cpp
\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 "chainiksolverpos_lma.hpp"
#include <iostream>
namespace KDL {
template <typename Derived>
inline void Twist_to_Eigen(const KDL::Twist& t,Eigen::MatrixBase<Derived>& e) {
e(0)=t.vel.data[0];
e(1)=t.vel.data[1];
e(2)=t.vel.data[2];
e(3)=t.rot.data[0];
e(4)=t.rot.data[1];
e(5)=t.rot.data[2];
}
ChainIkSolverPos_LMA::ChainIkSolverPos_LMA(
const KDL::Chain& _chain,
const Eigen::Matrix<double,6,1>& _L,
double _eps,
int _maxiter,
double _eps_joints
) :
lastNrOfIter(0),
lastSV(_chain.getNrOfJoints()),
jac(6, _chain.getNrOfJoints()),
grad(_chain.getNrOfJoints()),
display_information(false),
maxiter(_maxiter),
eps(_eps),
eps_joints(_eps_joints),
L(_L.cast<ScalarType>()),
chain(_chain),
T_base_jointroot(_chain.getNrOfJoints()),
T_base_jointtip(_chain.getNrOfJoints()),
q(_chain.getNrOfJoints()),
A(_chain.getNrOfJoints(), _chain.getNrOfJoints()),
tmp(_chain.getNrOfJoints()),
ldlt(_chain.getNrOfJoints()),
svd(6, _chain.getNrOfJoints(),Eigen::ComputeThinU | Eigen::ComputeThinV),
diffq(_chain.getNrOfJoints()),
q_new(_chain.getNrOfJoints()),
original_Aii(_chain.getNrOfJoints())
{}
ChainIkSolverPos_LMA::ChainIkSolverPos_LMA(
const KDL::Chain& _chain,
double _eps,
int _maxiter,
double _eps_joints
) :
lastNrOfIter(0),
lastSV(_chain.getNrOfJoints()>6?6:_chain.getNrOfJoints()),
jac(6, _chain.getNrOfJoints()),
grad(_chain.getNrOfJoints()),
display_information(false),
maxiter(_maxiter),
eps(_eps),
eps_joints(_eps_joints),
chain(_chain),
T_base_jointroot(_chain.getNrOfJoints()),
T_base_jointtip(_chain.getNrOfJoints()),
q(_chain.getNrOfJoints()),
A(_chain.getNrOfJoints(), _chain.getNrOfJoints()),
ldlt(_chain.getNrOfJoints()),
svd(6, _chain.getNrOfJoints(),Eigen::ComputeThinU | Eigen::ComputeThinV),
diffq(_chain.getNrOfJoints()),
q_new(_chain.getNrOfJoints()),
original_Aii(_chain.getNrOfJoints())
{
L(0)=1;
L(1)=1;
L(2)=1;
L(3)=0.01;
L(4)=0.01;
L(5)=0.01;
}
ChainIkSolverPos_LMA::~ChainIkSolverPos_LMA() {}
void ChainIkSolverPos_LMA::compute_fwdpos(const VectorXq& q) {
using namespace KDL;
unsigned int jointndx=0;
T_base_head = Frame::Identity(); // frame w.r.t. base of head
for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
const Segment& segment = chain.getSegment(i);
if (segment.getJoint().getType()!=Joint::None) {
T_base_jointroot[jointndx] = T_base_head;
T_base_head = T_base_head * segment.pose(q(jointndx));
T_base_jointtip[jointndx] = T_base_head;
jointndx++;
} else {
T_base_head = T_base_head * segment.pose(0.0);
}
}
}
void ChainIkSolverPos_LMA::compute_jacobian(const VectorXq& q) {
using namespace KDL;
unsigned int jointndx=0;
for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
const Segment& segment = chain.getSegment(i);
if (segment.getJoint().getType()!=Joint::None) {
// compute twist of the end effector motion caused by joint [jointndx]; expressed in base frame, with vel. ref. point equal to the end effector
KDL::Twist t = ( T_base_jointroot[jointndx].M * segment.twist(q(jointndx),1.0) ).RefPoint( T_base_head.p - T_base_jointtip[jointndx].p);
jac(0,jointndx)=t[0];
jac(1,jointndx)=t[1];
jac(2,jointndx)=t[2];
jac(3,jointndx)=t[3];
jac(4,jointndx)=t[4];
jac(5,jointndx)=t[5];
jointndx++;
}
}
}
void ChainIkSolverPos_LMA::display_jac(const KDL::JntArray& jval) {
VectorXq q;
q = jval.data.cast<ScalarType>();
compute_fwdpos(q);
compute_jacobian(q);
svd.compute(jac);
std::cout << "Singular values : " << svd.singularValues().transpose()<<"\n";
}
int ChainIkSolverPos_LMA::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out) {
using namespace KDL;
double v = 2;
double tau = 10;
double rho;
double lambda;
Twist t;
double delta_pos_norm;
Eigen::Matrix<ScalarType,6,1> delta_pos;
Eigen::Matrix<ScalarType,6,1> delta_pos_new;
q=q_init.data.cast<ScalarType>();
compute_fwdpos(q);
Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
delta_pos=L.asDiagonal()*delta_pos;
delta_pos_norm = delta_pos.norm();
if (delta_pos_norm<eps) {
lastNrOfIter =0 ;
Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
lastDifference = delta_pos.norm();
lastTransDiff = delta_pos.topRows(3).norm();
lastRotDiff = delta_pos.bottomRows(3).norm();
svd.compute(jac);
original_Aii = svd.singularValues();
lastSV = svd.singularValues();
q_out.data = q.cast<double>();
return 0;
}
compute_jacobian(q);
jac = L.asDiagonal()*jac;
lambda = tau;
double dnorm = 1;
for (unsigned int i=0;i<maxiter;++i) {
svd.compute(jac);
original_Aii = svd.singularValues();
for (unsigned int j=0;j<original_Aii.rows();++j) {
original_Aii(j) = original_Aii(j)/( original_Aii(j)*original_Aii(j)+lambda);
}
tmp = svd.matrixU().transpose()*delta_pos;
tmp = original_Aii.cwiseProduct(tmp);
diffq = svd.matrixV()*tmp;
grad = jac.transpose()*delta_pos;
if (display_information) {
std::cout << "------- iteration " << i << " ----------------\n"
<< " q = " << q.transpose() << "\n"
<< " weighted jac = \n" << jac << "\n"
<< " lambda = " << lambda << "\n"
<< " eigenvalues = " << svd.singularValues().transpose() << "\n"
<< " difference = " << delta_pos.transpose() << "\n"
<< " difference norm= " << delta_pos_norm << "\n"
<< " proj. on grad. = " << grad << "\n";
std::cout << std::endl;
}
dnorm = diffq.lpNorm<Eigen::Infinity>();
if (dnorm < eps_joints) {
lastDifference = delta_pos_norm;
lastNrOfIter = i;
lastSV = svd.singularValues();
q_out.data = q.cast<double>();
compute_fwdpos(q);
Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
lastTransDiff = delta_pos.topRows(3).norm();
lastRotDiff = delta_pos.bottomRows(3).norm();
return -2;
}
if (grad.transpose()*grad < eps_joints*eps_joints ) {
compute_fwdpos(q);
Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
lastDifference = delta_pos_norm;
lastTransDiff = delta_pos.topRows(3).norm();
lastRotDiff = delta_pos.bottomRows(3).norm();
lastSV = svd.singularValues();
lastNrOfIter = i;
q_out.data = q.cast<double>();
return -1;
}
q_new = q+diffq;
compute_fwdpos(q_new);
Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos_new );
delta_pos_new = L.asDiagonal()*delta_pos_new;
double delta_pos_new_norm = delta_pos_new.norm();
rho = delta_pos_norm*delta_pos_norm - delta_pos_new_norm*delta_pos_new_norm;
rho /= diffq.transpose()*(lambda*diffq + grad);
if (rho > 0) {
q = q_new;
delta_pos = delta_pos_new;
delta_pos_norm = delta_pos_new_norm;
if (delta_pos_norm<eps) {
Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
lastDifference = delta_pos_norm;
lastTransDiff = delta_pos.topRows(3).norm();
lastRotDiff = delta_pos.bottomRows(3).norm();
lastSV = svd.singularValues();
lastNrOfIter = i;
q_out.data = q.cast<double>();
return 0;
}
compute_jacobian(q_new);
jac = L.asDiagonal()*jac;
double tmp=2*rho-1;
lambda = lambda*max(1/3.0, 1-tmp*tmp*tmp);
v = 2;
} else {
lambda = lambda*v;
v = 2*v;
}
}
lastDifference = delta_pos_norm;
lastTransDiff = delta_pos.topRows(3).norm();
lastRotDiff = delta_pos.bottomRows(3).norm();
lastSV = svd.singularValues();
lastNrOfIter = maxiter;
q_out.data = q.cast<double>();
return -3;
}
};//namespace KDL

247
src/Mod/Robot/App/kdl_cp/chainiksolverpos_lma.hpp Normal file
View File

@ -0,0 +1,247 @@
#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:
/**
* \brief constructs an ChainIkSolverPos_LMA solver.
*
* The default parameters are choosen 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 0 if successful,
* -1 the gradient of \f$ E \f$ towards the joints is to small,
* -2 if joint position increments are to small,
* -3 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);
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;
const KDL::Chain& chain;
// 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

19
src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.cpp
View File

@ -38,20 +38,27 @@ namespace KDL
for(i=0;i<maxiter;i++){
fksolver.JntToCart(q_out,f);
delta_twist = diff(f,p_in);
iksolver.CartToJnt(q_out,delta_twist,delta_q);
const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q);
if (E_NOERROR > rc)
return (error = E_IKSOLVER_FAILED);
// we chose to continue if the child solver returned a positive
// "error", which may simply indicate a degraded solution
Add(q_out,delta_q,q_out);
if(Equal(delta_twist,Twist::Zero(),eps))
break;
// converged, but possibly with a degraded solution
return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR);
}
if(i!=maxiter)
return 0;
else
return -3;
return (error = E_NO_CONVERGE); // failed to converge
}
ChainIkSolverPos_NR::~ChainIkSolverPos_NR()
{
}
const char* ChainIkSolverPos_NR::strError(const int error) const
{
if (E_IKSOLVER_FAILED == error) return "Child IK solver failed";
else return SolverI::strError(error);
}
}

16
src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.hpp
View File

@ -38,6 +38,8 @@ namespace KDL {
class ChainIkSolverPos_NR : public ChainIkSolverPos
{
public:
static const int E_IKSOLVER_FAILED = -100; //! Child IK solver failed
/**
* Constructor of the solver, it needs the chain, a forward
* position kinematics solver and an inverse velocity
@ -57,8 +59,22 @@ namespace KDL {
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;
private:
const Chain chain;
ChainIkSolverVel& iksolver;

20
src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp
View File

@ -23,24 +23,14 @@
#include "chainiksolverpos_nr_jl.hpp"
#ifndef M_PI
#define M_PI 3.14159265358979323846 /* pi */
#endif
#ifndef M_PI_2
#define M_PI_2 1.57079632679489661923 /* pi/2 */
#endif
namespace KDL
{
ChainIkSolverPos_NR_JL::ChainIkSolverPos_NR_JL(const Chain& _chain, const JntArray& _q_min, const JntArray& _q_max, ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver,
unsigned int _maxiter, double _eps):
chain(_chain), q_min(chain.getNrOfJoints()), q_max(chain.getNrOfJoints()), iksolver(_iksolver), fksolver(_fksolver),delta_q(_chain.getNrOfJoints()),
chain(_chain), q_min(_q_min), q_max(_q_max), iksolver(_iksolver), fksolver(_fksolver), delta_q(_chain.getNrOfJoints()),
maxiter(_maxiter),eps(_eps)
{
q_min = _q_min;
q_max = _q_max;
}
int ChainIkSolverPos_NR_JL::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
@ -60,15 +50,13 @@ namespace KDL
for(unsigned int j=0; j<q_min.rows(); j++) {
if(q_out(j) < q_min(j))
//q_out(j) = q_min(j);
q_out(j) = q_out(j) + M_PI *2;
q_out(j) = q_min(j);
}
for(unsigned int j=0; j<q_max.rows(); j++) {
if(q_out(j) > q_max(j))
//q_out(j) = q_max(j);
q_out(j) = q_out(j) - M_PI *2;
q_out(j) = q_max(j);
}
}

7
src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.hpp
View File

@ -46,8 +46,8 @@ namespace KDL {
* kinematics solver for that chain.
*
* @param chain the chain to calculate the inverse position for
* @param q_max the maximum joint positions
* @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,
@ -69,11 +69,12 @@ namespace KDL {
ChainIkSolverVel& iksolver;
ChainFkSolverPos& fksolver;
JntArray delta_q;
unsigned int maxiter;
double eps;
Frame f;
Twist delta_twist;
unsigned int maxiter;
double eps;
};
}

44
src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv.cpp
View File

@ -33,7 +33,9 @@ namespace KDL
V(chain.getNrOfJoints(),JntArray(chain.getNrOfJoints())),
tmp(chain.getNrOfJoints()),
eps(_eps),
maxiter(_maxiter)
maxiter(_maxiter),
nrZeroSigmas(0),
svdResult(0)
{
}
@ -48,13 +50,21 @@ namespace KDL
//the current joint positions "q_in"
jnt2jac.JntToJac(q_in,jac);
double sum;
unsigned int i,j;
// Initialize near zero singular value counter
nrZeroSigmas = 0 ;
//Do a singular value decomposition of "jac" with maximum
//iterations "maxiter", put the results in "U", "S" and "V"
//jac = U*S*Vt
int ret = svd.calculate(jac,U,S,V,maxiter);
double sum;
unsigned int i,j;
svdResult = svd.calculate(jac,U,S,V,maxiter);
if (0 != svdResult)
{
qdot_out.data.setZero();
return (error = E_SVD_FAILED);
}
// We have to calculate qdot_out = jac_pinv*v_in
// Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
@ -68,7 +78,14 @@ namespace KDL
}
//If the singular value is too small (<eps), don't invert it but
//set the inverted singular value to zero (truncated svd)
tmp(i) = sum*(fabs(S(i))<eps?0.0:1.0/S(i));
if ( fabs(S(i))<eps ) {
tmp(i) = 0.0 ;
// Count number of singular values near zero
++nrZeroSigmas ;
}
else {
tmp(i) = sum/S(i) ;
}
}
//tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
//it with V to get qdot_out
@ -80,8 +97,19 @@ namespace KDL
//Put the result in qdot_out
qdot_out(i)=sum;
}
//return the return value of the svd decomposition
return ret;
// Note if the solution is singular, i.e. if number of near zero
// singular values is greater than the full rank of jac
if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular
} else {
return (error = E_NOERROR); // have converged
}
}
const char* ChainIkSolverVel_pinv::strError(const int error) const
{
if (E_SVD_FAILED == error) return "SVD failed";
else return SolverI::strError(error);
}
}

40
src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv.hpp
View File

@ -40,6 +40,10 @@ namespace KDL
class ChainIkSolverVel_pinv : public ChainIkSolverVel
{
public:
static const int E_SVD_FAILED = -100; //! Child SVD failed
/// solution converged but (pseudo)inverse is singular
static const int E_CONVERGE_PINV_SINGULAR = +100;
/**
* Constructor of the solver
*
@ -51,15 +55,47 @@ namespace KDL
* default: 150
*
*/
ChainIkSolverVel_pinv(const Chain& chain,double eps=0.00001,int maxiter=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 -1;};
/**
* 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;
private:
const Chain chain;
ChainJntToJacSolver jnt2jac;
@ -71,6 +107,8 @@ namespace KDL
JntArray tmp;
double eps;
int maxiter;
unsigned int nrZeroSigmas;
int svdResult;
};
}

20
src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.cpp
View File

@ -29,8 +29,8 @@ namespace KDL
jnt2jac(chain),
jac(chain.getNrOfJoints()),
transpose(chain.getNrOfJoints()>6),toggle(true),
m((int)max(6,chain.getNrOfJoints())),
n((int)min(6,chain.getNrOfJoints())),
m(max(6,chain.getNrOfJoints())),
n(min(6,chain.getNrOfJoints())),
jac_eigen(m,n),
U(MatrixXd::Identity(m,m)),
V(MatrixXd::Identity(n,n)),
@ -52,13 +52,15 @@ namespace KDL
int ChainIkSolverVel_pinv_givens::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
{
toggle=!toggle;
jnt2jac.JntToJac(q_in,jac);
for(unsigned int i=0;i<6;i++)
v_in_eigen(i)=v_in(i);
for(int i=0;i<m;i++){
for(int j=0;j<n;j++)
for(unsigned int i=0;i<m;i++){
for(unsigned int j=0;j<n;j++)
if(transpose)
jac_eigen(i,j)=jac(j,i);
else
@ -68,11 +70,11 @@ namespace KDL
//std::cout<<"# sweeps: "<<ret<<std::endl;
if(transpose)
UY = (V.transpose() * v_in_eigen).lazy();
UY.noalias() = V.transpose() * v_in_eigen;
else
UY = (U.transpose() * v_in_eigen).lazy();
UY.noalias() = U.transpose() * v_in_eigen;
for (int i = 0; i < n; i++){
for (unsigned int i = 0; i < n; i++){
double wi = UY(i);
double alpha = S(i);
@ -83,9 +85,9 @@ namespace KDL
SUY(i)= alpha * wi;
}
if(transpose)
qdot_eigen = (U * SUY).lazy();
qdot_eigen.noalias() = U * SUY;
else
qdot_eigen = (V * SUY).lazy();
qdot_eigen.noalias() = V * SUY;
for (unsigned int j=0;j<chain.getNrOfJoints();j++)
qdot_out(j)=qdot_eigen(j);

10
src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.hpp
View File

@ -8,7 +8,7 @@
#include <Eigen/Core>
USING_PART_OF_NAMESPACE_EIGEN;
using namespace Eigen;
namespace KDL
{
@ -29,9 +29,13 @@ namespace KDL
*
* @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
*
*/
ChainIkSolverVel_pinv_givens(const Chain& chain);
explicit ChainIkSolverVel_pinv_givens(const Chain& chain);
~ChainIkSolverVel_pinv_givens();
virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
@ -45,7 +49,7 @@ namespace KDL
ChainJntToJacSolver jnt2jac;
Jacobian jac;
bool transpose,toggle;
int m,n;
unsigned int m,n;
MatrixXd jac_eigen,U,V,B;
VectorXd S,tempi,tempj,UY,SUY,qdot_eigen,v_in_eigen;
};

165
src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.cpp
View File

@ -20,19 +20,21 @@
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#include "chainiksolvervel_pinv_nso.hpp"
#include "utilities/svd_eigen_HH.hpp"
namespace KDL
{
ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, JntArray _opt_pos, JntArray _weights, double _eps, int _maxiter, int _alpha):
ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, JntArray _opt_pos, JntArray _weights, double _eps, int _maxiter, double _alpha):
chain(_chain),
jnt2jac(chain),
jac(chain.getNrOfJoints()),
svd(jac),
U(6,JntArray(chain.getNrOfJoints())),
S(chain.getNrOfJoints()),
V(chain.getNrOfJoints(),JntArray(chain.getNrOfJoints())),
tmp(chain.getNrOfJoints()),
tmp2(chain.getNrOfJoints()-6),
nj(chain.getNrOfJoints()),
jac(nj),
U(MatrixXd::Zero(6,nj)),
S(VectorXd::Zero(nj)),
Sinv(VectorXd::Zero(nj)),
V(MatrixXd::Zero(nj,nj)),
tmp(VectorXd::Zero(nj)),
tmp2(VectorXd::Zero(nj)),
eps(_eps),
maxiter(_maxiter),
alpha(_alpha),
@ -41,16 +43,17 @@ namespace KDL
{
}
ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, double _eps, int _maxiter, int _alpha):
ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, double _eps, int _maxiter, double _alpha):
chain(_chain),
jnt2jac(chain),
jac(chain.getNrOfJoints()),
svd(jac),
U(6,JntArray(chain.getNrOfJoints())),
S(chain.getNrOfJoints()),
V(chain.getNrOfJoints(),JntArray(chain.getNrOfJoints())),
tmp(chain.getNrOfJoints()),
tmp2(chain.getNrOfJoints()-6),
nj(chain.getNrOfJoints()),
jac(nj),
U(MatrixXd::Zero(6,nj)),
S(VectorXd::Zero(nj)),
Sinv(VectorXd::Zero(nj)),
V(MatrixXd::Zero(nj,nj)),
tmp(VectorXd::Zero(nj)),
tmp2(VectorXd::Zero(nj)),
eps(_eps),
maxiter(_maxiter),
alpha(_alpha)
@ -71,76 +74,90 @@ namespace KDL
//Do a singular value decomposition of "jac" with maximum
//iterations "maxiter", put the results in "U", "S" and "V"
//jac = U*S*Vt
int ret = svd.calculate(jac,U,S,V,maxiter);
int svdResult = svd_eigen_HH(jac.data,U,S,V,tmp,maxiter);
if (0 != svdResult)
{
qdot_out.data.setZero() ;
return svdResult;
}
double sum;
unsigned int i,j;
unsigned int i;
// We have to calculate qdot_out = jac_pinv*v_in
// Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut):
// qdot_out = V*S_pinv*Ut*v_in
//first we calculate Ut*v_in
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.rows();j++) {
sum+= U[j](i)*v_in(j);
}
//If the singular value is too small (<eps), don't invert it but
//set the inverted singular value to zero (truncated svd)
tmp(i) = sum*(fabs(S(i))<eps?0.0:1.0/S(i));
// S^-1
for (i = 0; i < nj; ++i) {
Sinv(i) = fabs(S(i))<eps ? 0.0 : 1.0/S(i);
}
//tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
//it with V to get qdot_out
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.columns();j++) {
sum+=V[i](j)*tmp(j);
}
//Put the result in qdot_out
qdot_out(i)=sum;
for (i = 0; i < 6; ++i) {
tmp(i) = v_in(i);
}
//Now onto NULL space
for(i = 0; i < jac.columns(); i++)
tmp(i) = weights(i)*(opt_pos(i) - q_in(i));
//Vtn*tmp
for (i=jac.rows()+1;i<jac.columns();i++) {
tmp2(i-(jac.rows()+1)) = 0.0;
for (j=0;j<jac.columns();j++) {
tmp2(i-(jac.rows()+1)) +=V[j](i)*tmp(j);
}
qdot_out.data = V * Sinv.asDiagonal() * U.transpose() * tmp.head(6);
// Now onto NULL space
// Given the cost function g, and the current joints q, desired joints qd, and weights w:
// t = g(q) = 1/2 * Sum( w_i * (q_i - qd_i)^2 )
//
// The jacobian Jc is:
// t_dot = Jc(q) * q_dot
// Jc = dt/dq = w_j * (q_i - qd_i) [1 x nj vector]
//
// The pseudo inverse (^-1) is
// Jc^-1 = w_j * (q_i - qd_i) / A [nj x 1 vector]
// A = Sum( w_i^2 * (q_i - qd_i)^2 )
//
// We can set the change as the step needed to remove the error times a value alpha:
// t_dot = -2 * alpha * t
//
// When we put it together and project into the nullspace, the final result is
// q'_out += (I_n - J^-1 * J) * Jc^-1 * (-2 * alpha * g(q))
double g = 0; // g(q)
double A = 0; // normalizing term
for (i = 0; i < nj; ++i) {
double qd = q_in(i) - opt_pos(i);
g += 0.5 * qd*qd * weights(i);
A += qd*qd * weights(i)*weights(i);
}
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=jac.rows()+1;j<jac.columns();j++) {
sum +=V[i](j)*tmp2(j);
}
qdot_out(i) += alpha*sum;
if (A > 1e-9) {
// Calculate inverse Jacobian Jc^-1
for (i = 0; i < nj; ++i) {
tmp(i) = weights(i)*(q_in(i) - opt_pos(i)) / A;
}
// Calcualte J^-1 * J * Jc^-1 = V*S^-1*U' * U*S*V' * tmp
tmp2 = V * Sinv.asDiagonal() * U.transpose() * U * S.asDiagonal() * V.transpose() * tmp;
for (i = 0; i < nj; ++i) {
//std::cerr << i <<": "<< qdot_out(i) <<", "<< -2*alpha*g * (tmp(i) - tmp2(i)) << std::endl;
qdot_out(i) += -2*alpha*g * (tmp(i) - tmp2(i));
}
}
//return the return value of the svd decomposition
return ret;
return svdResult;
}
int ChainIkSolverVel_pinv_nso::setWeights(const JntArray & _weights)
{
weights = _weights;
return 0;
}
int ChainIkSolverVel_pinv_nso::setOptPos(const JntArray & _opt_pos)
{
opt_pos = _opt_pos;
return 0;
}
int ChainIkSolverVel_pinv_nso::setAlpha(const double _alpha)
{
alpha = _alpha;
return 0;
}
int ChainIkSolverVel_pinv_nso::setWeights(const JntArray & _weights)
{
weights = _weights;
return 0;
}
int ChainIkSolverVel_pinv_nso::setOptPos(const JntArray & _opt_pos)
{
opt_pos = _opt_pos;
return 0;
}
int ChainIkSolverVel_pinv_nso::setAlpha(const int _alpha)
{
alpha = _alpha;
return 0;
}
}

35
src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.hpp
View File

@ -19,19 +19,15 @@
// 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
#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_NSO_HPP
#define KDL_CHAIN_IKSOLVERVEL_PINV_NSO_HPP
#include "chainiksolver.hpp"
#include "chainjnttojacsolver.hpp"
#include "utilities/svd_HH.hpp"
#include <Eigen/Core>
namespace KDL
{
// FIXME: seems this class is unused/unmaintained/unfinished for several years
// it supposed to be either fixer or removed
/**
* Implementation of a inverse velocity kinematics algorithm based
* on the generalize pseudo inverse to calculate the velocity
@ -64,10 +60,8 @@ namespace KDL
* @param alpha the null-space velocity gain
*
*/
// FIXME: alpha is int but is initialized with a float value.
ChainIkSolverVel_pinv_nso(const Chain& chain, JntArray opt_pos, JntArray weights, double eps=0.00001,int maxiter=150, int alpha = 0.25);
ChainIkSolverVel_pinv_nso(const Chain& chain, double eps=0.00001,int maxiter=150, int alpha = 0.25);
ChainIkSolverVel_pinv_nso(const Chain& chain, JntArray opt_pos, 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);
@ -77,7 +71,6 @@ namespace KDL
*/
virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};
/**
*Set joint weights for optimization criterion
*
@ -100,25 +93,25 @@ namespace KDL
*@param alpha NUllspace velocity cgain
*
*/
virtual int setAlpha(const int alpha);
virtual int setAlpha(const double alpha);
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;
JntArray tmp2;
Eigen::MatrixXd U;
Eigen::VectorXd S;
Eigen::VectorXd Sinv;
Eigen::MatrixXd V;
Eigen::VectorXd tmp;
Eigen::VectorXd tmp2;
double eps;
int maxiter;
int alpha;
double alpha;
JntArray weights;
JntArray opt_pos;
};
}
#endif

101
src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp
View File

@ -42,7 +42,11 @@ namespace KDL
tmp_js(MatrixXd::Zero(chain.getNrOfJoints(),chain.getNrOfJoints())),
weight_ts(MatrixXd::Identity(6,6)),
weight_js(MatrixXd::Identity(chain.getNrOfJoints(),chain.getNrOfJoints())),
lambda(0.0)
lambda(0.0),
lambda_scaled(0.0),
nrZeroSigmas(0),
svdResult(0),
sigmaMin(0)
{
}
@ -50,19 +54,29 @@ namespace KDL
{
}
void ChainIkSolverVel_wdls::setWeightJS(const Eigen::MatrixXd& Mq){
void ChainIkSolverVel_wdls::setWeightJS(const MatrixXd& Mq){
weight_js = Mq;
}
void ChainIkSolverVel_wdls::setWeightTS(const Eigen::MatrixXd& Mx){
void ChainIkSolverVel_wdls::setWeightTS(const MatrixXd& Mx){
weight_ts = Mx;
}
void ChainIkSolverVel_wdls::setLambda(const double& lambda_in)
void ChainIkSolverVel_wdls::setLambda(const double lambda_in)
{
lambda=lambda_in;
}
void ChainIkSolverVel_wdls::setEps(const double eps_in)
{
eps=eps_in;
}
void ChainIkSolverVel_wdls::setMaxIter(const int maxiter_in)
{
maxiter=maxiter_in;
}
int ChainIkSolverVel_wdls::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out)
{
jnt2jac.JntToJac(q_in,jac);
@ -70,6 +84,11 @@ namespace KDL
double sum;
unsigned int i,j;
// Initialize (internal) return values
nrZeroSigmas = 0 ;
sigmaMin = 0.;
lambda_scaled = 0.;
/*
for (i=0;i<jac.rows();i++) {
for (j=0;j<jac.columns();j++)
@ -78,17 +97,30 @@ namespace KDL
*/
// Create the Weighted jacobian
tmp_jac_weight1 = (jac.data*weight_js).lazy();
tmp_jac_weight2 = (weight_ts*tmp_jac_weight1).lazy();
tmp_jac_weight1 = jac.data.lazyProduct(weight_js);
tmp_jac_weight2 = weight_ts.lazyProduct(tmp_jac_weight1);
// Compute the SVD of the weighted jacobian
int ret = svd_eigen_HH(tmp_jac_weight2,U,S,V,tmp,maxiter);
svdResult = svd_eigen_HH(tmp_jac_weight2,U,S,V,tmp,maxiter);
if (0 != svdResult)
{
qdot_out.data.setZero() ;
return (error = E_SVD_FAILED);
}
//Pre-multiply U and V by the task space and joint space weighting matrix respectively
tmp_ts = (weight_ts*U.corner(Eigen::TopLeft,6,6)).lazy();
tmp_js = (weight_js*V).lazy();
// tmp = (Si*U'*Ly*y),
tmp_ts = weight_ts.lazyProduct(U.topLeftCorner(6,6));
tmp_js = weight_js.lazyProduct(V);
// Minimum of six largest singular values of J is S(5) if number of joints >=6 and 0 for <6
if ( jac.columns() >= 6 ) {
sigmaMin = S(5);
}
else {
sigmaMin = 0.;
}
// tmp = (Si*U'*Ly*y),
for (i=0;i<jac.columns();i++) {
sum = 0.0;
for (j=0;j<jac.rows();j++) {
@ -97,11 +129,29 @@ namespace KDL
else
sum+=0.0;
}
if(S(i)==0||S(i)<eps)
tmp(i) = sum*((S(i)/(S(i)*S(i)+lambda*lambda)));
else
// If sigmaMin > eps, then wdls is not active and lambda_scaled = 0 (default value)
// If sigmaMin < eps, then wdls is active and lambda_scaled is scaled from 0 to lambda
// Note: singular values are always positive so sigmaMin >=0
if ( sigmaMin < eps )
{
lambda_scaled = sqrt(1.0-(sigmaMin/eps)*(sigmaMin/eps))*lambda ;
}
if(fabs(S(i))<eps) {
if (i<6) {
// Scale lambda to size of singular value sigmaMin
tmp(i) = sum*((S(i)/(S(i)*S(i)+lambda_scaled*lambda_scaled)));
}
else {
tmp(i)=0.0; // S(i)=0 for i>=6 due to cols>rows
}
// Count number of singular values near zero
++nrZeroSigmas ;
}
else {
tmp(i) = sum/S(i);
}
}
/*
// x = Lx^-1*V*tmp + x
for (i=0;i<jac.columns();i++) {
@ -112,8 +162,21 @@ namespace KDL
qdot_out(i)=sum;
}
*/
qdot_out.data=(tmp_js*tmp).lazy();
return ret;
qdot_out.data=tmp_js.lazyProduct(tmp);
// If number of near zero singular values is greater than the full rank
// of jac, then wdls is active
if ( nrZeroSigmas > (jac.columns()-jac.rows()) ) {
return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular
} else {
return (error = E_NOERROR); // have converged
}
}
const char* ChainIkSolverVel_wdls::strError(const int error) const
{
if (E_SVD_FAILED == error) return "SVD failed";
else return SolverI::strError(error);
}
}

80
src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.hpp
View File

@ -63,6 +63,10 @@ namespace KDL
class ChainIkSolverVel_wdls : public ChainIkSolverVel
{
public:
static const int E_SVD_FAILED = -100; //! SVD solver failed
/// solution converged but (pseudo)inverse is singular
static const int E_CONVERGE_PINV_SINGULAR = +100;
/**
* Constructor of the solver
*
@ -75,10 +79,25 @@ namespace KDL
*
*/
ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=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.
@ -89,9 +108,8 @@ namespace KDL
/**
* Set the joint space weighting matrix
*
* weight_js joint space weighting symetric matrix,
* default : identity.
* @param Mq : This matrix being used as a
* @param weight_js joint space weighting symetric 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
@ -114,9 +132,8 @@ namespace KDL
/**
* Set the task space weighting matrix
*
* weight_ts task space weighting symetric matrix,
* default: identity
* @param Mx : This matrix being used as a weight
* @param weight_ts task space weighting symetric 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
@ -137,7 +154,50 @@ namespace KDL
*/
void setWeightTS(const Eigen::MatrixXd& Mx);
void setLambda(const double& lambda);
/**
* 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 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;
private:
const Chain chain;
@ -157,6 +217,10 @@ namespace KDL
Eigen::MatrixXd weight_ts;
Eigen::MatrixXd weight_js;
double lambda;
double lambda_scaled;
unsigned int nrZeroSigmas ;
int svdResult;
double sigmaMin;
};
}
#endif

29
src/Mod/Robot/App/kdl_cp/chainjnttojacsolver.cpp
View File

@ -43,19 +43,32 @@ namespace KDL
if(!locked_joints_[i])
nr_of_unlocked_joints_++;
}
return 0;
}
int ChainJntToJacSolver::JntToJac(const JntArray& q_in,Jacobian& jac)
int ChainJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, int seg_nr)
{
if(q_in.rows()!=chain.getNrOfJoints()||nr_of_unlocked_joints_!=static_cast<int>(jac.columns()))
return -1;
unsigned int segmentNr;
if(seg_nr<0)
segmentNr=chain.getNrOfSegments();
else
segmentNr = seg_nr;
//Initialize Jacobian to zero since only segmentNr colunns are computed
SetToZero(jac) ;
if(q_in.rows()!=chain.getNrOfJoints()||nr_of_unlocked_joints_!=jac.columns())
return (error = E_JAC_FAILED);
else if(segmentNr>chain.getNrOfSegments())
return (error = E_JAC_FAILED);
T_tmp = Frame::Identity();
SetToZero(t_tmp);
int j=0;
int k=0;
Frame total;
for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
for (unsigned int i=0;i<segmentNr;i++) {
//Calculate new Frame_base_ee
if(chain.getSegment(i).getJoint().getType()!=Joint::None){
//pose of the new end-point expressed in the base
@ -82,7 +95,13 @@ namespace KDL
T_tmp = total;
}
return 0;
return (error = E_NOERROR);
}
const char* ChainJntToJacSolver::strError(const int error) const
{
if (E_JAC_FAILED == error) return "Jac Failed";
else return SolverI::strError(error);
}
}

17
src/Mod/Robot/App/kdl_cp/chainjnttojacsolver.hpp
View File

@ -22,6 +22,7 @@
#ifndef KDL_CHAINJNTTOJACSOLVER_HPP
#define KDL_CHAINJNTTOJACSOLVER_HPP
#include "solveri.hpp"
#include "frames.hpp"
#include "jacobian.hpp"
#include "jntarray.hpp"
@ -37,11 +38,13 @@ namespace KDL
*
*/
class ChainJntToJacSolver
class ChainJntToJacSolver : public SolverI
{
public:
ChainJntToJacSolver(const Chain& chain);
~ChainJntToJacSolver();
static const int E_JAC_FAILED = -100; //! Jac solver failed
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
@ -53,15 +56,19 @@ namespace KDL
*
* @return always returns 0
*/
int JntToJac(const JntArray& q_in,Jacobian& jac);
virtual int JntToJac(const JntArray& q_in, Jacobian& jac, int segmentNR=-1);
int setLockedJoints(const std::vector<bool> locked_joints);
/// @copydoc KDL::SolverI::strError()
virtual const char* strError(const int error) const;
private:
const Chain chain;
Twist t_tmp;
Frame T_tmp;
std::vector<bool> locked_joints_;
int nr_of_unlocked_joints_;
unsigned int nr_of_unlocked_joints_;
};
}
#endif

37
src/Mod/Robot/App/kdl_cp/config.h.in 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 @KDL_VERSION_MAJOR@
#define KDL_VERSION_MINOR @KDL_VERSION_MINOR@
#define KDL_VERSION_PATCH @KDL_VERSION_PATCH@
#define KDL_VERSION (KDL_VERSION_MAJOR << 16) | (KDL_VERSION_MINOR << 8) | KDL_VERSION_PATCH
#define KDL_VERSION_STRING "@KDL_VERSION@"
//Set which version of the Tree Interface to use
#cmakedefine HAVE_STL_CONTAINER_INCOMPLETE_TYPES
#cmakedefine KDL_USE_NEW_TREE_INTERFACE
#endif //#define KDL_CONFIG_H

19
src/Mod/Robot/App/kdl_cp/frameacc.hpp
View File

@ -40,6 +40,23 @@ 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
{
@ -244,13 +261,13 @@ public:
}
#ifdef KDL_INLINE
#include "frameacc.inl"
#endif
}

3
src/Mod/Robot/App/kdl_cp/frameacc.inl
View File

@ -17,7 +17,6 @@
****************************************************************************/
namespace KDL {
/////////////////// VectorAcc /////////////////////////////////////
@ -431,6 +430,7 @@ Twist FrameAcc::GetAccTwist() const {
TwistAcc TwistAcc::Zero()
{
return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero());
@ -596,4 +596,3 @@ bool Equal(const TwistAcc& a,const Twist& b,double eps) {
Equal(a.vel,b.vel,eps) );
}
}

102
src/Mod/Robot/App/kdl_cp/frames.cpp
View File

@ -27,6 +27,9 @@
#include "frames.hpp"
#define _USE_MATH_DEFINES // For MSVC
#include <math.h>
namespace KDL {
#ifndef KDL_INLINE
@ -40,7 +43,7 @@ namespace KDL {
for (i=0;i<3;i++) {
for (j=0;j<3;j++)
d[i*4+j]=M(i,j);
d[i*4+3] = p(i)/1000;
d[i*4+3] = p(i);
}
for (j=0;j<3;j++)
d[12+j] = 0.;
@ -194,32 +197,33 @@ namespace KDL {
*/
void Rotation::GetQuaternion(double& x,double& y,double& z, double& w) const
{
double trace = (*this)(0,0) + (*this)(1,1) + (*this)(2,2) + 1.0f;
double trace = (*this)(0,0) + (*this)(1,1) + (*this)(2,2);
double epsilon=1E-12;
if( trace > epsilon ){
double s = 0.5f / sqrt(trace);
w = 0.25f / s;
double s = 0.5 / sqrt(trace + 1.0);
w = 0.25 / s;
x = ( (*this)(2,1) - (*this)(1,2) ) * s;
y = ( (*this)(0,2) - (*this)(2,0) ) * s;
z = ( (*this)(1,0) - (*this)(0,1) ) * s;
}else{
if ( (*this)(0,0) > (*this)(1,1) && (*this)(0,0) > (*this)(2,2) ){
float s = 2.0f * sqrtf( 1.0f + (*this)(0,0) - (*this)(1,1) - (*this)(2,2));
double s = 2.0 * sqrt( 1.0 + (*this)(0,0) - (*this)(1,1) - (*this)(2,2));
w = ((*this)(2,1) - (*this)(1,2) ) / s;
x = 0.25f * s;
x = 0.25 * s;
y = ((*this)(0,1) + (*this)(1,0) ) / s;
z = ((*this)(0,2) + (*this)(2,0) ) / s;
} else if ((*this)(1,1) > (*this)(2,2)) {
float s = 2.0f * sqrtf( 1.0f + (*this)(1,1) - (*this)(0,0) - (*this)(2,2));
double s = 2.0 * sqrt( 1.0 + (*this)(1,1) - (*this)(0,0) - (*this)(2,2));
w = ((*this)(0,2) - (*this)(2,0) ) / s;
x = ((*this)(0,1) + (*this)(1,0) ) / s;
y = 0.25f * s;
y = 0.25 * s;
z = ((*this)(1,2) + (*this)(2,1) ) / s;
}else {
float s = 2.0f * sqrtf( 1.0f + (*this)(2,2) - (*this)(0,0) - (*this)(1,1) );
double s = 2.0 * sqrt( 1.0 + (*this)(2,2) - (*this)(0,0) - (*this)(1,1) );
w = ((*this)(1,0) - (*this)(0,1) ) / s;
x = ((*this)(0,2) + (*this)(2,0) ) / s;
y = ((*this)(1,2) + (*this)(2,1) ) / s;
z = 0.25f * s;
z = 0.25 * s;
}
}
}
@ -238,13 +242,13 @@ Rotation Rotation::RPY(double roll,double pitch,double yaw)
// Gives back a rotation matrix specified with RPY convention
void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const
{
if (fabs(data[6]) > 1.0 - epsilon ) {
roll = -sign(data[6]) * atan2(data[1], data[4]);
pitch= -sign(data[6]) * PI / 2;
yaw = 0.0 ;
double epsilon=1E-12;
pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) );
if ( fabs(pitch) > (M_PI/2.0-epsilon) ) {
yaw = atan2( -data[1], data[4]);
roll = 0.0 ;
} else {
roll = atan2(data[7], data[8]);
pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) );
yaw = atan2(data[3], data[0]);
}
}
@ -262,18 +266,19 @@ Rotation Rotation::EulerZYZ(double Alfa,double Beta,double Gamma) {
}
void Rotation::GetEulerZYZ(double& alfa,double& beta,double& gamma) const {
if (fabs(data[6]) < epsilon ) {
alfa=0.0;
void Rotation::GetEulerZYZ(double& alpha,double& beta,double& gamma) const {
double epsilon = 1E-12;
if (fabs(data[8]) > 1-epsilon ) {
gamma=0.0;
if (data[8]>0) {
beta = 0.0;
gamma= atan2(-data[1],data[0]);
alpha= atan2(data[3],data[0]);
} else {
beta = PI;
gamma= atan2(data[1],-data[0]);
alpha= atan2(-data[3],-data[0]);
}
} else {
alfa=atan2(data[5], data[2]);
alpha=atan2(data[5], data[2]);
beta=atan2(sqrt( sqr(data[6]) +sqr(data[7]) ),data[8]);
gamma=atan2(data[7], -data[6]);
}
@ -326,18 +331,10 @@ Vector Rotation::GetRot() const
// Returns a vector with the direction of the equiv. axis
// and its norm is angle
{
Vector axis = Vector((data[7]-data[5]),
(data[2]-data[6]),
(data[3]-data[1]) )/2;
double sa = axis.Norm();
double ca = (data[0]+data[4]+data[8]-1)/2.0;
double alfa;
if (sa > epsilon)
alfa = ::atan2(sa,ca)/sa;
else
alfa = 1;
return axis * alfa;
Vector axis;
double angle;
angle = Rotation::GetRotAngle(axis,epsilon);
return axis * angle;
}
@ -345,7 +342,7 @@ Vector Rotation::GetRot() const
/** Returns the rotation angle around the equiv. axis
* @param axis the rotation axis is returned in this variable
* @param eps : in the case of angle == 0 : rot axis is undefined and choosen
* to be +/- Z-axis
* to be the Z-axis
* in the case of angle == PI : 2 solutions, positive Z-component
* of the axis is choosen.
* @result returns the rotation angle (between [0..PI] )
@ -354,30 +351,43 @@ Vector Rotation::GetRot() const
*/
double Rotation::GetRotAngle(Vector& axis,double eps) const {
double ca = (data[0]+data[4]+data[8]-1)/2.0;
if (ca>1-eps) {
double t= eps*eps/2.0;
if (ca>1-t) {
// undefined choose the Z-axis, and angle 0
axis = Vector(0,0,1);
return 0;
}
if (ca < -1+eps) {
if (ca < -1+t) {
// The case of angles consisting of multiples of M_PI:
// two solutions, choose a positive Z-component of the axis
double z = sqrt( (data[8]+1)/2 );
double x = (data[2])/2/z;
double y = (data[5])/2/z;
double x = sqrt( (data[0]+1.0)/2);
double y = sqrt( (data[4]+1.0)/2);
double z = sqrt( (data[8]+1.0)/2);
if ( data[2] < 0) x=-x;
if ( data[7] < 0) y=-y;
if ( x*y*data[1] < 0) x=-x; // this last line can be necessary when z is 0
// z always >= 0
// if z equal to zero
axis = Vector( x,y,z );
return PI;
}
double angle = acos(ca);
double sa = sin(angle);
axis = Vector((data[7]-data[5])/2/sa,
(data[2]-data[6])/2/sa,
(data[3]-data[1])/2/sa );
return angle;
double angle;
double mod_axis;
double axisx, axisy, axisz;
axisx = data[7]-data[5];
axisy = data[2]-data[6];
axisz = data[3]-data[1];
mod_axis = sqrt(axisx*axisx+axisy*axisy+axisz*axisz);
axis = Vector(axisx/mod_axis,
axisy/mod_axis,
axisz/mod_axis );
angle = atan2(mod_axis/2,ca);
return angle;
}
bool operator==(const Rotation& a,const Rotation& b) {
#ifdef KDL_USE_EQUAL
return Equal(a,b,epsilon);
return Equal(a,b);
#else
return ( a.data[0]==b.data[0] &&
a.data[1]==b.data[1] &&

271
src/Mod/Robot/App/kdl_cp/frames.hpp
View File

@ -144,6 +144,15 @@ class Rotation2;
class Frame2;
// Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4)
inline bool Equal(const Vector& a,const Vector& b,double eps=epsilon);
inline bool Equal(const Frame& a,const Frame& b,double eps=epsilon);
inline bool Equal(const Twist& a,const Twist& b,double eps=epsilon);
inline bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon);
inline bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon);
inline bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon);
inline bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon);
/**
* \brief A concrete implementation of a 3 dimensional vector class
@ -378,24 +387,34 @@ public:
double GetRotAngle(Vector& axis,double eps=epsilon) const;
//! Gives back a rotation matrix specified with EulerZYZ convention :
//! First rotate around Z with alfa,
//! then around the new Y with beta, then around
//! new Z with gamma.
/** Gives back a rotation matrix specified with EulerZYZ convention :
* - First rotate around Z with alfa,
* - then around the new Y with beta,
* - then around new Z with gamma.
* Invariants:
* - EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PHI, -beta, gamma +/- PI)
* - (angle + 2*k*PI)
**/
static Rotation EulerZYZ(double Alfa,double Beta,double Gamma);
//! Gives back the EulerZYZ convention description of the rotation matrix :
//! First rotate around Z with alfa,
//! then around the new Y with beta, then around
//! new Z with gamma.
//!
//! Variables are bound by
//! (-PI <= alfa <= PI),
//! (0 <= beta <= PI),
//! (-PI <= alfa <= PI)
void GetEulerZYZ(double& alfa,double& beta,double& gamma) const;
/** Gives back the EulerZYZ convention description of the rotation matrix :
First rotate around Z with alpha,
then around the new Y with beta, then around
new Z with gamma.
//! Sets the value of this object to a rotation specified with Quaternion convention
Variables are bound by:
- (-PI < alpha <= PI),
- (0 <= beta <= PI),
- (-PI < gamma <= PI)
if beta==0 or beta==PI, then alpha and gamma are not unique, in this case gamma is chosen to be zero.
Invariants:
- EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PI, -beta, gamma +/- PI)
- angle + 2*k*PI
*/
void GetEulerZYZ(double& alpha,double& beta,double& gamma) const;
//! Gives back a rotation matrix specified with Quaternion convention
//! the norm of (x,y,z,w) should be equal to 1
static Rotation Quaternion(double x,double y,double z, double w);
@ -403,42 +422,74 @@ public:
//! \post the norm of (x,y,z,w) is 1
void GetQuaternion(double& x,double& y,double& z, double& w) const;
//! Sets the value of this object to a rotation specified with RPY convention:
//! first rotate around X with roll, then around the
//! old Y with pitch, then around old Z with alfa
/**
*
* Gives back a rotation matrix specified with RPY convention:
* first rotate around X with roll, then around the
* old Y with pitch, then around old Z with yaw
*
* Invariants:
* - RPY(roll,pitch,yaw) == RPY( roll +/- PI, PI-pitch, yaw +/- PI )
* - angles + 2*k*PI
*/
static Rotation RPY(double roll,double pitch,double yaw);
//! Gives back a vector in RPY coordinates, variables are bound by
//! -PI <= roll <= PI
//! -PI <= Yaw <= PI
//! -PI/2 <= PITCH <= PI/2
//!
//! convention : first rotate around X with roll, then around the
//! old Y with pitch, then around old Z with alfa
/** Gives back a vector in RPY coordinates, variables are bound by
- -PI <= roll <= PI
- -PI <= Yaw <= PI
- -PI/2 <= PITCH <= PI/2
convention :
- first rotate around X with roll,
- then around the old Y with pitch,
- then around old Z with yaw
if pitch == PI/2 or pitch == -PI/2, multiple solutions for gamma and alpha exist. The solution where roll==0
is chosen.
Invariants:
- RPY(roll,pitch,yaw) == RPY( roll +/- PI, PI-pitch, yaw +/- PI )
- angles + 2*k*PI
**/
void GetRPY(double& roll,double& pitch,double& yaw) const;
//! Gives back a rotation matrix specified with EulerZYX convention :
//! First rotate around Z with alfa,
//! then around the new Y with beta, then around
//! new X with gamma.
//!
//! closely related to RPY-convention
/** EulerZYX constructs a Rotation from the Euler ZYX parameters:
* - First rotate around Z with alfa,
* - then around the new Y with beta,
* - then around new X with gamma.
*
* Closely related to RPY-convention.
*
* Invariants:
* - EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PI, PI-beta, gamma +/- PI)
* - (angle + 2*k*PI)
**/
inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) {
return RPY(Gamma,Beta,Alfa);
}
//! GetEulerZYX gets the euler ZYX parameters of a rotation :
//! First rotate around Z with alfa,
//! then around the new Y with beta, then around
//! new X with gamma.
//!
//! Range of the results of GetEulerZYX :
//! -PI <= alfa <= PI
//! -PI <= gamma <= PI
//! -PI/2 <= beta <= PI/2
//!
//! Closely related to RPY-convention.
/** GetEulerZYX gets the euler ZYX parameters of a rotation :
* First rotate around Z with alfa,
* then around the new Y with beta, then around
* new X with gamma.
*
* Range of the results of GetEulerZYX :
* - -PI <= alfa <= PI
* - -PI <= gamma <= PI
* - -PI/2 <= beta <= PI/2
*
* if beta == PI/2 or beta == -PI/2, multiple solutions for gamma and alpha exist. The solution where gamma==0
* is chosen.
*
*
* Invariants:
* - EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PI, PI-beta, gamma +/- PI)
* - and also (angle + 2*k*PI)
*
* Closely related to RPY-convention.
**/
inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const {
GetRPY(Gamma,Beta,Alfa);
}
@ -503,6 +554,7 @@ public:
friend class Frame;
};
bool operator==(const Rotation& a,const Rotation& b);
bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon);
@ -587,9 +639,9 @@ public:
//! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()).
inline static Frame Identity();
//! The twist \<t_this\> is expressed wrt the current
//! The twist <t_this> is expressed wrt the current
//! frame. This frame is integrated into an updated frame with
//! \<samplefrequency\>. Very simple first order integration rule.
//! <samplefrequency>. Very simple first order integration rule.
inline void Integrate(const Twist& t_this,double frequency);
/*
@ -1078,18 +1130,124 @@ public:
inline friend bool Equal(const Frame2& a,const Frame2& b,double eps);
};
IMETHOD Vector diff(const Vector& a,const Vector& b,double dt=1);
IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1);
IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1);
IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1);
IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1);
IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt=1);
IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt=1);
IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt=1);
IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1);
IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);
/**
* determines the difference of vector b with vector a.
*
* see diff for Rotation matrices for further background information.
*
* \param p_w_a start vector a expressed to some frame w
* \param p_w_b end vector b expressed to some frame w .
* \param dt [optional][obsolete] time interval over which the numerical differentiation takes place.
* \return the difference (b-a) expressed to the frame w.
*/
IMETHOD Vector diff(const Vector& p_w_a,const Vector& p_w_b,double dt=1);
} // namespace KDL
/**
* determines the (scaled) rotation axis necessary to rotate from b1 to b2.
*
* This rotation axis is expressed w.r.t. frame a. The rotation axis is scaled
* by the necessary rotation angle. The rotation angle is always in the
* (inclusive) interval \f$ [0 , \pi] \f$.
*
* This definition is chosen in this way to facilitate numerical differentiation.
* With this definition diff(a,b) == -diff(b,a).
*
* The diff() function is overloaded for all classes in frames.hpp and framesvel.hpp, such that
* numerical differentiation, equality checks with tolerances, etc. can be performed
* without caring exactly on which type the operation is performed.
*
* \param R_a_b1: The rotation matrix \f$ _a^{b1} R \f$ of b1 with respect to frame a.
* \param R_a_b2: The Rotation matrix \f$ _a^{b2} R \f$ of b2 with respect to frame a.
* \param dt [optional][obsolete] time interval over which the numerical differentiation takes place. By default this is set to 1.0.
* \return rotation axis to rotate from b1 to b2, scaled by the rotation angle, expressed in frame a.
* \warning - The result is not a rotational vector, i.e. it is not a mathematical vector.
* (no communitative addition).
*
* \warning - When used in the context of numerical differentiation, with the frames b1 and b2 very
* close to each other, the semantics correspond to the twist, scaled by the time.
*
* \warning - For angles equal to \f$ \pi \f$, The negative of the
* return value is equally valid.
*/
IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1);
/**
* determines the rotation axis necessary to rotate the frame b1 to the same orientation as frame b2 and the vector
* necessary to translate the origin of b1 to the origin of b2, and stores the result in a Twist datastructure.
* \param F_a_b1 frame b1 expressed with respect to some frame a.
* \param F_a_b2 frame b2 expressed with respect to some frame a.
* \warning The result is not a Twist!
* see diff() for Rotation and Vector arguments for further detail on the semantics.
*/
IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1);
/**
* determines the difference between two twists i.e. the difference between
* the underlying velocity vectors and rotational velocity vectors.
*/
IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1);
/**
* determines the difference between two wrenches i.e. the difference between
* the underlying torque vectors and force vectors.
*/
IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1);
/**
* \brief adds vector da to vector a.
* see also the corresponding diff() routine.
* \param p_w_a vector a expressed to some frame w.
* \param p_w_da vector da expressed to some frame w.
* \returns the vector resulting from the displacement of vector a by vector da, expressed in the frame w.
*/
IMETHOD Vector addDelta(const Vector& p_w_a,const Vector& p_w_da,double dt=1);
/**
* returns the rotation matrix resulting from the rotation of frame a by the axis and angle
* specified with da_w.
*
* see also the corresponding diff() routine.
*
* \param R_w_a Rotation matrix of frame a expressed to some frame w.
* \param da_w axis and angle of the rotation expressed to some frame w.
* \returns the rotation matrix resulting from the rotation of frame a by the axis and angle
* specified with da. The resulting rotation matrix is expressed with respect to
* frame w.
*/
IMETHOD Rotation addDelta(const Rotation& R_w_a,const Vector& da_w,double dt=1);
/**
* returns the frame resulting from the rotation of frame a by the axis and angle
* specified in da_w and the translation of the origin (also specified in da_w).
*
* see also the corresponding diff() routine.
* \param R_w_a Rotation matrix of frame a expressed to some frame w.
* \param da_w axis and angle of the rotation (da_w.rot), together with a displacement vector for the origin (da_w.vel), expressed to some frame w.
* \returns the frame resulting from the rotation of frame a by the axis and angle
* specified with da.rot, and the translation of the origin da_w.vel . The resulting frame is expressed with respect to frame w.
*/
IMETHOD Frame addDelta(const Frame& F_w_a,const Twist& da_w,double dt=1);
/**
* \brief adds the twist da to the twist a.
* see also the corresponding diff() routine.
* \param a a twist wrt some frame
* \param da a twist difference wrt some frame
* \returns The twist (a+da) wrt the corresponding frame.
*/
IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1);
/**
* \brief adds the wrench da to the wrench w.
* see also the corresponding diff() routine.
* see also the corresponding diff() routine.
* \param a a wrench wrt some frame
* \param da a wrench difference wrt some frame
* \returns the wrench (a+da) wrt the corresponding frame.
*/
IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);
#ifdef KDL_INLINE
// #include "vector.inl"
@ -1105,4 +1263,7 @@ IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);
}
#endif

58
src/Mod/Robot/App/kdl_cp/frames.inl
View File

@ -25,7 +25,12 @@
* *
***************************************************************************/
namespace KDL {
/**
* \file frames.inl
* Inlined member functions and global functions that relate to the classes in frames.cpp
*
*/
IMETHOD Vector::Vector(const Vector & arg)
@ -1114,47 +1119,15 @@ IMETHOD Rotation Rot(const Vector& axis_a_b) {
ct + vt*rotvec(2)*rotvec(2)
);
}
IMETHOD Vector diff(const Vector& a,const Vector& b,double dt) {
return (b-a)/dt;
}
/**
* \brief diff operator for displacement rotational velocity.
*
* The Vector arguments here represent a displacement rotational velocity. i.e. a rotation
* around a fixed axis for a certain angle. For this representation you cannot use diff() but
* have to use diff_displ().
*
* \todo represent a displacement twist and displacement rotational velocity
* with another class, instead of Vector and Twist.
* \warning do not confuse displacement rotational velocities and velocities
* \warning do not confuse displacement twist and twist.
*
IMETHOD Vector diff_displ(const Vector& a,const Vector& b,double dt) {
return diff(Rot(a),Rot(b),dt);
}*/
/**
* \brief diff operator for displacement twist.
*
* The Twist arguments here represent a displacement twist. i.e. a rotation
* around a fixed axis for a certain angle. For this representation you cannot use diff() but
* have to use diff_displ().
*
* \warning do not confuse displacement rotational velocities and velocities
* \warning do not confuse displacement twist and twist.
*
IMETHOD Twist diff_displ(const Twist& a,const Twist& b,double dt) {
return Twist(diff(a.vel,b.vel,dt),diff(Rot(a.rot),Rot(b.rot),dt));
}
*/
IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt) {
Rotation R_b1_b2(R_a_b1.Inverse()*R_a_b2);
return R_a_b1 * R_b1_b2.GetRot() / dt;
}
IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt) {
return Twist(
diff(F_a_b1.p,F_a_b2.p,dt),
@ -1194,7 +1167,7 @@ IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt) {
}
/* Commented code section
/**
* \brief addDelta operator for displacement rotational velocity.
*
* The Vector arguments here represent a displacement rotational velocity. i.e. a rotation
@ -1212,7 +1185,7 @@ IMETHOD Vector addDelta_displ(const Vector& a,const Vector&da,double dt) {
return getRot(addDelta(Rot(a),da,dt));
}*/
/* Commented code section
/**
* \brief addDelta operator for displacement twist.
*
* The Vector arguments here represent a displacement rotational velocity. i.e. a rotation
@ -1294,7 +1267,7 @@ IMETHOD void posrandom(Frame& F) {
IMETHOD bool operator==(const Frame& a,const Frame& b ) {
#ifdef KDL_USE_EQUAL
return Equal(a,b,epsilon);
return Equal(a,b);
#else
return (a.p == b.p &&
a.M == b.M );
@ -1307,7 +1280,7 @@ IMETHOD bool operator!=(const Frame& a,const Frame& b) {
IMETHOD bool operator==(const Vector& a,const Vector& b) {
#ifdef KDL_USE_EQUAL
return Equal(a,b,epsilon);
return Equal(a,b);
#else
return (a.data[0]==b.data[0]&&
a.data[1]==b.data[1]&&
@ -1321,7 +1294,7 @@ IMETHOD bool operator!=(const Vector& a,const Vector& b) {
IMETHOD bool operator==(const Twist& a,const Twist& b) {
#ifdef KDL_USE_EQUAL
return Equal(a,b,epsilon);
return Equal(a,b);
#else
return (a.rot==b.rot &&
a.vel==b.vel );
@ -1334,7 +1307,7 @@ IMETHOD bool operator!=(const Twist& a,const Twist& b) {
IMETHOD bool operator==(const Wrench& a,const Wrench& b ) {
#ifdef KDL_USE_EQUAL
return Equal(a,b,epsilon);
return Equal(a,b);
#else
return (a.force==b.force &&
a.torque==b.torque );
@ -1350,7 +1323,7 @@ IMETHOD bool operator!=(const Rotation& a,const Rotation& b) {
IMETHOD bool operator==(const Vector2& a,const Vector2& b) {
#ifdef KDL_USE_EQUAL
return Equal(a,b,epsilon);
return Equal(a,b);
#else
return (a.data[0]==b.data[0]&&
a.data[1]==b.data[1] );
@ -1361,6 +1334,3 @@ IMETHOD bool operator!=(const Vector2& a,const Vector2& b) {
return !operator==(a,b);
}
} // namespace KDL

20
src/Mod/Robot/App/kdl_cp/framevel.hpp
View File

@ -52,7 +52,7 @@ IMETHOD void posrandom(doubleVel& F) {
posrandom(F.grad);
}
} //namespace KDL
}
template <>
struct Traits<KDL::doubleVel> {
@ -67,6 +67,20 @@ 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
@ -369,12 +383,12 @@ IMETHOD void posrandom(FrameVel& F) {
posrandom(F.p);
}
} // namespace KDL
#ifdef KDL_INLINE
#include "framevel.inl"
#endif
} // namespace
#endif

3
src/Mod/Robot/App/kdl_cp/framevel.inl
View File

@ -16,7 +16,6 @@
* $Name: $
****************************************************************************/
namespace KDL {
// Methods and operators related to FrameVelVel
// They all delegate most of the work to RotationVelVel and VectorVelVel
@ -533,5 +532,3 @@ Twist TwistVel::GetTwist() const {
Twist TwistVel::GetTwistDot() const {
return Twist(vel.v,rot.v);
}
} // namespace KDL

10
src/Mod/Robot/App/kdl_cp/jacobian.cpp
View File

@ -23,7 +23,7 @@
namespace KDL
{
USING_PART_OF_NAMESPACE_EIGEN
using namespace Eigen;
Jacobian::Jacobian()
{
@ -126,12 +126,12 @@ namespace KDL
bool Jacobian::operator ==(const Jacobian& arg)const
{
return Equal((*this),arg,epsilon);
return Equal((*this),arg);
}
bool Jacobian::operator!=(const Jacobian& arg)const
{
return !Equal((*this),arg,epsilon);
return !Equal((*this),arg);
}
bool Equal(const Jacobian& a,const Jacobian& b,double eps)
@ -147,8 +147,8 @@ namespace KDL
}
void Jacobian::setColumn(unsigned int i,const Twist& t){
data.col(i).start<3>()=Eigen::Map<Vector3d>(t.vel.data);
data.col(i).end<3>()=Eigen::Map<Vector3d>(t.rot.data);
data.col(i).head<3>()=Eigen::Map<const Vector3d>(t.vel.data);
data.col(i).tail<3>()=Eigen::Map<const Vector3d>(t.rot.data);
}
}

9
src/Mod/Robot/App/kdl_cp/jacobian.hpp
View File

@ -27,12 +27,19 @@
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);
class Jacobian
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::Matrix<double,6,Eigen::Dynamic> data;
Jacobian();
Jacobian(unsigned int nr_of_columns);
explicit Jacobian(unsigned int nr_of_columns);
Jacobian(const Jacobian& arg);
///Allocates memory for new size (can break realtime behavior)

4
src/Mod/Robot/App/kdl_cp/jntarray.cpp
View File

@ -23,7 +23,7 @@
namespace KDL
{
USING_PART_OF_NAMESPACE_EIGEN
using namespace Eigen;
JntArray::JntArray()
{
@ -101,7 +101,7 @@ namespace KDL
void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest)
{
Eigen::Matrix<double,6,1> t=(jac.data*src.data).lazy();
Eigen::Matrix<double,6,1> t=jac.data.lazyProduct(src.data);
dest=Twist(Vector(t(0),t(1),t(2)),Vector(t(3),t(4),t(5)));
}

14
src/Mod/Robot/App/kdl_cp/jntarray.hpp
View File

@ -89,7 +89,7 @@ class MyTask : public RTT::TaskContext
* @post 0 < rows()
* @post all elements in data have 0 value
*/
JntArray(unsigned int size);
explicit JntArray(unsigned int size);
/** Copy constructor
* @note Will correctly copy an empty object
*/
@ -139,18 +139,6 @@ class MyTask : public RTT::TaskContext
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);
/**
* 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 == (KDL::Twist::)Zero() if 0 == src.rows() (ie src is empty)
*/
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);

22
src/Mod/Robot/App/kdl_cp/jntarrayacc.hpp
View File

@ -29,6 +29,23 @@
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:
@ -37,10 +54,10 @@ namespace KDL
JntArray qdotdot;
public:
JntArrayAcc(){};
JntArrayAcc(unsigned int size);
explicit JntArrayAcc(unsigned int size);
JntArrayAcc(const JntArray& q,const JntArray& qdot,const JntArray& qdotdot);
JntArrayAcc(const JntArray& q,const JntArray& qdot);
JntArrayAcc(const JntArray& q);
explicit JntArrayAcc(const JntArray& q);
void resize(unsigned int newSize);
@ -64,6 +81,7 @@ namespace KDL
friend bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps);
};
}
#endif

18
src/Mod/Robot/App/kdl_cp/jntarrayvel.hpp
View File

@ -28,6 +28,19 @@
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
{
@ -36,9 +49,9 @@ namespace KDL
JntArray qdot;
public:
JntArrayVel(){};
JntArrayVel(unsigned int size);
explicit JntArrayVel(unsigned int size);
JntArrayVel(const JntArray& q,const JntArray& qdot);
JntArrayVel(const JntArray& q);
explicit JntArrayVel(const JntArray& q);
void resize(unsigned int newSize);
@ -57,6 +70,7 @@ namespace KDL
friend bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps);
};
}
#endif

6
src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.cpp
View File

@ -23,7 +23,7 @@
namespace KDL
{
USING_PART_OF_NAMESPACE_EIGEN
using namespace Eigen;
JntSpaceInertiaMatrix::JntSpaceInertiaMatrix()
{
@ -100,7 +100,7 @@ namespace KDL
void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest)
{
dest.data=(src.data*vec.data).lazy();
dest.data=src.data.lazyProduct(vec.data);
}
void SetToZero(JntSpaceInertiaMatrix& mat)
@ -115,7 +115,7 @@ namespace KDL
return src1.data.isApprox(src2.data,eps);
}
bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2,epsilon);};
bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);};
//bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);};
}

153
src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp
View File

@ -63,9 +63,9 @@ class MyTask : public RTT::TaskContext
** use j here
}
};
\endcode
/endcode
*/
*/
class JntSpaceInertiaMatrix
{
@ -90,7 +90,7 @@ class MyTask : public RTT::TaskContext
* @post 0 < rows()
* @post all elements in data have 0 value
*/
JntSpaceInertiaMatrix(int size);
explicit JntSpaceInertiaMatrix(int size);
/** Copy constructor
* @note Will correctly copy an empty object
*/
@ -134,84 +134,99 @@ class MyTask : public RTT::TaskContext
*/
unsigned int columns()const;
/**
* 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
*/
friend 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
*/
friend 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
*/
friend 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
*/
friend 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 src J: Inertia Matrix (6x6)
* @param vec q: Jacobian (6x1)
* @param dest t: Twist (6x1)
* @post dest==Twist::Zero() if 0==src.rows() (ie src is empty)
*/
friend void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest);
/**
* Function to set all the values of the array to 0
*
* @param mat
*/
friend void SetToZero(JntSpaceInertiaMatrix& mat);
/**
* 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())
*/
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

48
src/Mod/Robot/App/kdl_cp/joint.cpp
View File

@ -29,6 +29,7 @@ namespace KDL {
name(_name),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
{
if (type == RotAxis || type == TransAxis) throw joint_type_ex;
q_previous = 0;
}
// constructor for joint along x,y or z axis, at origin of reference frame
@ -37,12 +38,14 @@ namespace KDL {
name("NoName"),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
{
if (type == RotAxis || type == TransAxis) throw joint_type_ex;
q_previous = 0;
}
// constructor for joint along arbitrary axis, at arbitrary origin
Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale,
const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness), axis(_axis / _axis.Norm()), origin(_origin)
name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
, axis(_axis / _axis.Norm()), origin(_origin)
{
if (type != RotAxis && type != TransAxis) throw joint_type_ex;
@ -55,7 +58,8 @@ namespace KDL {
// constructor for joint along arbitrary axis, at arbitrary origin
Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale,
const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness), axis(_axis / _axis.Norm()), origin(_origin)
name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness),
axis(_axis / _axis.Norm()),origin(_origin)
{
if (type != RotAxis && type != TransAxis) throw joint_type_ex;
@ -71,42 +75,32 @@ namespace KDL {
Frame Joint::pose(const double& q)const
{
switch(type){
case RotAxis:{
case RotAxis:
// calculate the rotation matrix around the vector "axis"
if (q != q_previous){
q_previous = q;
joint_pose.M = Rotation::Rot2(axis, scale*q+offset);
}
return joint_pose;
break;}
case RotX:
return Frame(Rotation::RotX(scale*q+offset));
break;
case RotY:
return Frame(Rotation::RotY(scale*q+offset));
break;
case RotZ:
return Frame(Rotation::RotZ(scale*q+offset));
break;
case TransAxis:
return Frame(origin + (axis * (scale*q+offset)));
break;
case TransX:
return Frame(Vector(scale*q+offset,0.0,0.0));
break;
case TransY:
return Frame(Vector(0.0,scale*q+offset,0.0));
break;
case TransZ:
return Frame(Vector(0.0,0.0,scale*q+offset));
break;
case None:
default:
return Frame::Identity();
break;
}
return Frame::Identity();
}
Twist Joint::twist(const double& qdot)const
@ -114,33 +108,24 @@ namespace KDL {
switch(type){
case RotAxis:
return Twist(Vector(0,0,0), axis * ( scale * qdot));
break;
case RotX:
return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
break;
case RotY:
return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
break;
case RotZ:
return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
break;
case TransAxis:
return Twist(axis * (scale * qdot), Vector(0,0,0));
break;
case TransAxis:
return Twist(axis * (scale * qdot), Vector(0,0,0));
case TransX:
return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0));
break;
case TransY:
return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0));
break;
case TransZ:
return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
break;
case None:
default:
return Twist::Zero();
break;
}
return Twist::Zero();
}
Vector Joint::JointAxis() const
@ -149,33 +134,24 @@ namespace KDL {
{
case RotAxis:
return axis;
break;
case RotX:
return Vector(1.,0.,0.);
break;
case RotY:
return Vector(0.,1.,0.);
break;
case RotZ:
return Vector(0.,0.,1.);
break;
case TransAxis:
return axis;
break;
case TransX:
return Vector(1.,0.,0.);
break;
case TransY:
return Vector(0.,1.,0.);
break;
case TransZ:
return Vector(0.,0.,1.);
break;
case None:
default:
return Vector::Zero();
break;
}
return Vector::Zero();
}
Vector Joint::JointOrigin() const

36
src/Mod/Robot/App/kdl_cp/joint.hpp
View File

@ -59,7 +59,7 @@ namespace KDL {
* @param stiffness 1D stiffness along the joint axis,
* default: 0
*/
Joint(const std::string& name, const JointType& type=None,const double& scale=1,const double& offset=0,
explicit Joint(const std::string& name, const JointType& type=None,const double& scale=1,const double& offset=0,
const double& inertia=0,const double& damping=0,const double& stiffness=0);
/**
* Constructor of a joint.
@ -74,22 +74,21 @@ namespace KDL {
* @param stiffness 1D stiffness along the joint axis,
* default: 0
*/
Joint(const JointType& type=None,const double& scale=1,const double& offset=0,
explicit Joint(const JointType& type=None,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 type type of the joint
* @param _scale scale between joint input and actual geometric
* @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
* @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,
* @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,
@ -97,16 +96,15 @@ namespace KDL {
/**
* Constructor of a joint.
*
* @param _origin the origin of the joint
* @param _axis the axis of the joint
* @param type type of the joint
* @param _scale scale between joint input and actual geometric
* @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
* @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,
* @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,
@ -205,7 +203,7 @@ namespace KDL {
double damping;
double stiffness;
// varibles for RotAxis joint
// variables for RotAxis joint
Vector axis, origin;
mutable Frame joint_pose;
mutable double q_previous;

103
src/Mod/Robot/App/kdl_cp/kdl.hpp
View File

@ -1,4 +1,5 @@
// 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>
@ -20,25 +21,109 @@
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
/**
* \defgroup KDL Kinematics and Dynamics Library
* \ingroup ROBOT
* \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:
* <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).
*
* Geometric primitives and their transformations:
*
**/
/**
* \page geomprim Geometric Primitives
* \section Introduction
* Geometric primitives are represented by the following classes.
* - KDL::Vector
* - KDL::Rotation
* - KDL::Frame
* - KDL::Twist
* - KDL::Wrench
*
* Other modules:
* - \ref KinFam : 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.
* \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.
*
*/
/* This code doesn't seems to be integrated with freecad
- \ref KDLTK : the interface code to integrate KDL into the Orocos <a href="http://www.orocos.org/rtt/">Real-Time Toolkit</a> (RTT). */

11
src/Mod/Robot/App/kdl_cp/kdl.pc.in Normal file
View File

@ -0,0 +1,11 @@
prefix=@CMAKE_INSTALL_PREFIX@
libdir=${prefix}/lib
includedir=${prefix}/include
Name: orocos-kdl
Description: The Orocos Kinematics and Dynamics Library
Requires:
Version: @KDL_VERSION@
Libs: -L${libdir} -lorocos-kdl
Cflags: -I${includedir} @KDL_CFLAGS@

3
src/Mod/Robot/App/kdl_cp/kinfam.hpp
View File

@ -20,8 +20,7 @@
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
/**
* @defgroup KinFam Kinematic Families
* \ingroup KDL
* @defgroup KinematicFamily Kinematic Families
* @brief All classes to support kinematic families.
*
* The Kinematic Families classes range from the basic building blocks

13
src/Mod/Robot/App/kdl_cp/kinfam_io.cpp
View File

@ -24,7 +24,8 @@
namespace KDL {
std::ostream& operator <<(std::ostream& os, const Joint& joint) {
return os << joint.getTypeName();
return os << joint.getName()<<":["<<joint.getTypeName()
<<", axis: "<<joint.JointAxis() << ", origin"<<joint.JointOrigin()<<"]";
}
std::istream& operator >>(std::istream& is, Joint& joint) {
@ -32,7 +33,7 @@ std::istream& operator >>(std::istream& is, Joint& joint) {
}
std::ostream& operator <<(std::ostream& os, const Segment& segment) {
os << "[" << segment.getJoint() << ",\n" << segment.getFrameToTip() << "]";
os << segment.getName()<<":[" << segment.getJoint() << ",\n tip: \n" << segment.getFrameToTip() << "]";
return os;
}
@ -53,15 +54,15 @@ std::istream& operator >>(std::istream& is, Chain& chain) {
}
std::ostream& operator <<(std::ostream& os, const Tree& tree) {
SegmentMap::const_iterator root = tree.getSegment("root");
SegmentMap::const_iterator root = tree.getRootSegment();
return os << root;
}
std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator root) {
//os<<root->first<<": "<<root->second.segment<<"\n";
os << root->first<<"(q_nr: "<<root->second.q_nr<<")"<<"\n \t";
for (unsigned int i = 0; i < root->second.children.size(); i++) {
os <<(root->second.children[i])<<"\t";
os << root->first<<"(q_nr: "<< GetTreeElementQNr(root->second) << ")" << "\n \t";
for (unsigned int i = 0; i < GetTreeElementChildren(root->second).size(); i++) {
os << ( GetTreeElementChildren(root->second)[i] ) << "\t";
}
return os << "\n";
}

1
src/Mod/Robot/App/kdl_cp/motion.hpp
View File

@ -21,7 +21,6 @@
/**
* @defgroup Motion Motion
* \ingroup KDL
* @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.

14
src/Mod/Robot/App/kdl_cp/path.hpp
View File

@ -58,7 +58,14 @@ namespace KDL {
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
@ -115,6 +122,11 @@ class Path
*/
virtual Path* Clone() = 0;
/**
* gets an identifier indicating the type of this Path object
*/
virtual IdentifierType getIdentifier() const=0;
virtual ~Path() {}
};

6
src/Mod/Robot/App/kdl_cp/path_circle.cpp
View File

@ -65,14 +65,12 @@ Path_Circle::Path_Circle(const Frame& F_base_start,
Vector x(F_base_start.p - F_base_center.p);
radius = x.Normalize();
if (radius < epsilon)
throw Error_MotionPlanning_Circle_ToSmall();
if (radius < epsilon) throw Error_MotionPlanning_Circle_ToSmall();
Vector tmpv(V_base_p-F_base_center.p);
tmpv.Normalize();
Vector z( x * tmpv);
double n = z.Normalize();
if (n < epsilon)
throw Error_MotionPlanning_Circle_No_Plane();
if (n < epsilon) throw Error_MotionPlanning_Circle_No_Plane();
F_base_center.M = Rotation(x,z*x,z);
double dist = alpha*radius;
// See what has the slowest eq. motion, and adapt

8
src/Mod/Robot/App/kdl_cp/path_circle.hpp
View File

@ -100,6 +100,14 @@ class Path_Circle : public Path
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();
};

29
src/Mod/Robot/App/kdl_cp/path_composite.cpp
View File

@ -53,7 +53,8 @@ namespace KDL {
// you propably want to use the cached_index variable
double Path_Composite::Lookup(double s) const
{
assert(s>=-1e-12);
assert(s<=pathlength+1e-12);
if ( (cached_starts <=s) && ( s <= cached_ends) ) {
return s - cached_starts;
}
@ -92,6 +93,7 @@ double Path_Composite::PathLength() {
return pathlength;
}
Frame Path_Composite::Pos(double s) const {
s = Lookup(s);
return gv[cached_index].first->Pos(s);
@ -124,6 +126,29 @@ void Path_Composite::Write(std::ostream& os) {
os << "]" << std::endl;
}
int Path_Composite::GetNrOfSegments() {
return dv.size();
}
Path* Path_Composite::GetSegment(int i) {
assert(i>=0);
assert(i<dv.size());
return gv[i].first;
}
double Path_Composite::GetLengthToEndOfSegment(int i) {
assert(i>=0);
assert(i<dv.size());
return dv[i];
}
void Path_Composite::GetCurrentSegmentLocation(double s, int& segment_number,
double& inner_s)
{
inner_s = Lookup(s);
segment_number= cached_index;
}
Path_Composite::~Path_Composite() {
PathVector::iterator it;
for (it=gv.begin();it!=gv.end();++it) {
@ -132,4 +157,4 @@ Path_Composite::~Path_Composite() {
}
}
}
} // namespace KDL

47
src/Mod/Robot/App/kdl_cp/path_composite.hpp
View File

@ -53,6 +53,17 @@ 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
@ -113,6 +124,42 @@ namespace KDL {
*/
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();
};

6
src/Mod/Robot/App/kdl_cp/path_cyclic_closed.hpp
View File

@ -73,6 +73,12 @@ namespace KDL {
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();
};

7
src/Mod/Robot/App/kdl_cp/path_line.hpp
View File

@ -121,6 +121,13 @@ class Path_Line : public Path
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();
};

7
src/Mod/Robot/App/kdl_cp/path_point.hpp
View File

@ -73,6 +73,13 @@ class Path_Point : public Path
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();
};

144
src/Mod/Robot/App/kdl_cp/path_roundedcomposite.cpp
View File

@ -49,71 +49,104 @@
namespace KDL {
// private constructor, to keep the type when cloning a Path_RoundedComposite, such that getIdentifier keeps on returning
// the correct value:
Path_RoundedComposite::Path_RoundedComposite(Path_Composite* _comp,
double _radius, double _eqradius, RotationalInterpolation* _orient,
bool _aggregate,int _nrofpoints):
comp(_comp), radius(_radius), eqradius(_eqradius), orient(_orient), nrofpoints(_nrofpoints), aggregate(_aggregate) {
}
Path_RoundedComposite::Path_RoundedComposite(double _radius,double _eqradius,RotationalInterpolation* _orient, bool _aggregate) :
comp( new Path_Composite()), radius(_radius),eqradius(_eqradius), orient(_orient), aggregate(_aggregate)
{
nrofpoints = 0;
if (eqradius<=0) {
throw Error_MotionPlanning_Not_Feasible(1);
}
}
void Path_RoundedComposite::Add(const Frame& F_base_point) {
if (nrofpoints==0) {
double eps = 1E-7;
if (nrofpoints == 0) {
F_base_start = F_base_point;
} else if (nrofpoints==1) {
F_base_via = F_base_point;
} else if (nrofpoints == 1) {
F_base_via = F_base_point;
} else {
// calculate rounded segment : line + circle,
// determine the angle between the line segments :
Vector ab = F_base_via.p - F_base_start.p;
Vector bc = F_base_point.p - F_base_via.p;
double abdist = ab.Normalize();
double alpha = ::acos(dot(ab,bc)/(ab.Norm()*bc.Norm()));
//double alpha = ::acos(dot(ab,bc));
double d = radius/tan((PI-alpha)/2);
double bcdist = bc.Normalize();
Vector ab = F_base_via.p - F_base_start.p;
Vector bc = F_base_point.p - F_base_via.p;
double abdist = ab.Norm();
double bcdist = bc.Norm();
if (abdist < eps) {
throw Error_MotionPlanning_Not_Feasible(2);
}
if (bcdist < eps) {
throw Error_MotionPlanning_Not_Feasible(3);
}
double alpha = acos(dot(ab, bc) / abdist / bcdist);
if ((PI - alpha) < eps) {
throw Error_MotionPlanning_Not_Feasible(4);
}
if (alpha < eps) {
// no rounding is done in the case of parallel line segments
comp->Add(
new Path_Line(F_base_start, F_base_via, orient->Clone(),
eqradius));
F_base_start = F_base_via;
F_base_via = F_base_point;
} else {
double d = radius / tan((PI - alpha) / 2); // tan. is guaranteed not to return zero.
if (d >= abdist)
throw Error_MotionPlanning_Not_Feasible();
throw Error_MotionPlanning_Not_Feasible(5);
if (d >= bcdist)
throw Error_MotionPlanning_Not_Feasible();
std::auto_ptr<Path> line1 (
new Path_Line(F_base_start,F_base_via,orient->Clone(),eqradius)
);
std::auto_ptr<Path> line2 (
new Path_Line(F_base_via,F_base_point,orient->Clone(),eqradius)
);
Frame F_base_circlestart = line1->Pos(line1->LengthToS(abdist-d));
Frame F_base_circleend = line2->Pos(line2->LengthToS(d));
// end of circle segment, beginning of next line
Vector V_base_t = ab*(ab*bc);
throw Error_MotionPlanning_Not_Feasible(6);
std::auto_ptr < Path
> line1(
new Path_Line(F_base_start, F_base_via,
orient->Clone(), eqradius));
std::auto_ptr < Path
> line2(
new Path_Line(F_base_via, F_base_point,
orient->Clone(), eqradius));
Frame F_base_circlestart = line1->Pos(line1->LengthToS(abdist - d));
Frame F_base_circleend = line2->Pos(line2->LengthToS(d));
// end of circle segment, beginning of next line
Vector V_base_t = ab * (ab * bc);
V_base_t.Normalize();
comp->Add(new Path_Line(
F_base_start,F_base_circlestart,orient->Clone(),eqradius
)
);
comp->Add(new Path_Circle(
F_base_circlestart,
F_base_circlestart.p - V_base_t * radius,
F_base_circleend.p,
F_base_circleend.M,
PI-alpha,orient->Clone(),eqradius
)
);
// shift for next line
F_base_start = F_base_circleend; // end of the circle segment
F_base_via = F_base_point;
comp->Add(
new Path_Line(F_base_start, F_base_circlestart,
orient->Clone(), eqradius));
comp->Add(
new Path_Circle(F_base_circlestart,
F_base_circlestart.p - V_base_t * radius,
F_base_circleend.p, F_base_circleend.M, alpha,
orient->Clone(), eqradius));
// shift for next line
F_base_start = F_base_circleend; // end of the circle segment
F_base_via = F_base_point;
}
}
nrofpoints++;
}
void Path_RoundedComposite::Finish() {
if (nrofpoints>=1) {
comp->Add(new Path_Line(F_base_start,F_base_via,orient->Clone(),eqradius));
if (nrofpoints >= 1) {
comp->Add(
new Path_Line(F_base_start, F_base_via, orient->Clone(),
eqradius));
}
}
double Path_RoundedComposite::LengthToS(double length) {
double Path_RoundedComposite::LengthToS(double length) {
return comp->LengthToS(length);
}
double Path_RoundedComposite::PathLength() {
return comp->PathLength();
}
@ -122,18 +155,37 @@ Frame Path_RoundedComposite::Pos(double s) const {
return comp->Pos(s);
}
Twist Path_RoundedComposite::Vel(double s,double sd) const {
return comp->Vel(s,sd);
Twist Path_RoundedComposite::Vel(double s, double sd) const {
return comp->Vel(s, sd);
}
Twist Path_RoundedComposite::Acc(double s,double sd,double sdd) const {
return comp->Acc(s,sd,sdd);
Twist Path_RoundedComposite::Acc(double s, double sd, double sdd) const {
return comp->Acc(s, sd, sdd);
}
void Path_RoundedComposite::Write(std::ostream& os) {
void Path_RoundedComposite::Write(std::ostream& os) {
comp->Write(os);
}
int Path_RoundedComposite::GetNrOfSegments() {
return comp->GetNrOfSegments();
}
Path* Path_RoundedComposite::GetSegment(int i) {
return comp->GetSegment(i);
}
double Path_RoundedComposite::GetLengthToEndOfSegment(int i) {
return comp->GetLengthToEndOfSegment(i);
}
void Path_RoundedComposite::GetCurrentSegmentLocation(double s,
int& segment_number, double& inner_s) {
comp->GetCurrentSegmentLocation(s,segment_number,inner_s);
}
Path_RoundedComposite::~Path_RoundedComposite() {
if (aggregate)
delete orient;
@ -142,7 +194,7 @@ Path_RoundedComposite::~Path_RoundedComposite() {
Path* Path_RoundedComposite::Clone() {
return comp->Clone();
return new Path_RoundedComposite(static_cast<Path_Composite*>(comp->Clone()),radius,eqradius,orient->Clone(), true, nrofpoints);
}
}

69
src/Mod/Robot/App/kdl_cp/path_roundedcomposite.hpp
View File

@ -51,8 +51,8 @@
namespace KDL {
/**
* The specification of a path, composed of
* way-points with rounded corners.
* The specification of a path, composed of way-points with rounded corners.
*
* @ingroup Motion
*/
class Path_RoundedComposite : public Path
@ -73,21 +73,38 @@ class Path_RoundedComposite : public Path
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 : default True
* @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);
Path_RoundedComposite(double radius,double eqradius,RotationalInterpolation* orient, bool aggregate=true);
/**
* Adds a point to this rounded composite, between to adjecent points
* a Path_Line will be created, between two lines there will be
* rounding with the given radius with a Path_Circle
* Can throw Error_MotionPlanning_Not_Feasible object
*
* 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 M_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);
@ -108,6 +125,7 @@ class Path_RoundedComposite : public Path
*/
virtual double PathLength();
/**
* Returns the Frame at the current path length s
*/
@ -137,6 +155,43 @@ class Path_RoundedComposite : public Path
*/
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();
};

16
src/Mod/Robot/App/kdl_cp/rigidbodyinertia.cpp
View File

@ -30,15 +30,15 @@ namespace KDL{
const static bool mhi=true;
RigidBodyInertia::RigidBodyInertia(double m_,const Vector& h_,const RotationalInertia& I_,bool mhi):
m(m_),I(I_),h(h_)
m(m_),h(h_),I(I_)
{
}
RigidBodyInertia::RigidBodyInertia(double m_, const Vector& c_, const RotationalInertia& Ic):
m(m_),h(m*c_){
//I=Ic-c x c x
Vector3d c_eig=Map<Vector3d>(c_.data);
Map<Matrix3d>(I.data)=Map<Matrix3d>(Ic.data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Matrix3d::Identity());
Vector3d c_eig=Map<const Vector3d>(c_.data);
Map<Matrix3d>(I.data)=Map<const Matrix3d>(Ic.data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Matrix3d::Identity());
}
RigidBodyInertia operator*(double a,const RigidBodyInertia& I){
@ -60,13 +60,13 @@ namespace KDL{
//Ib = R(Ia+r x h x + (h-m*r) x r x)R'
Vector hmr = (I.h-I.m*X.p);
Vector3d r_eig = Map<Vector3d>(X.p.data);
Vector3d h_eig = Map<Vector3d>(I.h.data);
Vector3d h_eig = Map<const Vector3d>(I.h.data);
Vector3d hmr_eig = Map<Vector3d>(hmr.data);
Matrix3d rcrosshcross = h_eig *r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity();
Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity();
Matrix3d R = Map<Matrix3d>(X.M.data);
RotationalInertia Ib;
Map<Matrix3d>(Ib.data) = R*((Map<Matrix3d>(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose());
Map<Matrix3d>(Ib.data) = R*((Map<const Matrix3d>(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose());
return RigidBodyInertia(I.m,T.M*hmr,Ib,mhi);
}
@ -75,9 +75,9 @@ namespace KDL{
//mb=ma
//hb=R*h
//Ib = R(Ia)R' with r=0
Matrix3d R = Map<Matrix3d>(M.data);
Matrix3d R = Map<const Matrix3d>(M.data);
RotationalInertia Ib;
Map<Matrix3d>(Ib.data) = R.transpose()*(Map<Matrix3d>(I.I.data)*R);
Map<Matrix3d>(Ib.data) = R.transpose()*(Map<const Matrix3d>(I.I.data)*R);
return RigidBodyInertia(I.m,M*I.h,Ib,mhi);
}
@ -87,7 +87,7 @@ namespace KDL{
//hb=(h-m*r)
//Ib = (Ia+r x h x + (h-m*r) x r x)
Vector hmr = (this->h-this->m*p);
Vector3d r_eig = Map<Vector3d>(p.data);
Vector3d r_eig = Map<const Vector3d>(p.data);
Vector3d h_eig = Map<Vector3d>(this->h.data);
Vector3d hmr_eig = Map<Vector3d>(hmr.data);
Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity();

55
src/Mod/Robot/App/kdl_cp/rigidbodyinertia.hpp
View File

@ -41,7 +41,7 @@ namespace KDL {
* 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.
*/
RigidBodyInertia(double m=0, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero());
explicit RigidBodyInertia(double m=0, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero());
/**
* Creates an inertia with zero mass, and zero RotationalInertia
@ -53,31 +53,10 @@ namespace KDL {
~RigidBodyInertia(){};
/**
* Scalar product: I_new = double * I_old
*/
friend 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
*/
friend 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
*/
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);
/**
* Reference frame orientation change Ia = R_a_b*Ib with R_a_b
* the rotation of b expressed in a
*/
friend RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I);
/**
@ -111,12 +90,42 @@ namespace KDL {
private:
RigidBodyInertia(double m,const Vector& h,const RotationalInertia& I,bool mhi);
double m;
RotationalInertia I;
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

2
src/Mod/Robot/App/kdl_cp/rotational_interpolation.hpp
View File

@ -105,7 +105,7 @@ class RotationalInterpolation
static RotationalInterpolation* Read(std::istream& is);
/**
* virtual constructor, construction by copying .
* virtual constructor, construction by copying ..
*/
virtual RotationalInterpolation* Clone() const = 0;

4
src/Mod/Robot/App/kdl_cp/rotational_interpolation_sa.hpp
View File

@ -56,8 +56,8 @@ 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 choosen, therefore it is better to
* not try to interpolate a 180 degrees rotation (but it will 'work').
* exist, an arbitrary one will be choosen, therefore it is not recommended
* to try to interpolate a 180 degrees rotation.
* @ingroup Motion
*/
class RotationalInterpolation_SingleAxis: public RotationalInterpolation

6
src/Mod/Robot/App/kdl_cp/rotationalinertia.cpp
View File

@ -43,19 +43,19 @@ namespace KDL
Vector RotationalInertia::operator*(const Vector& omega) const {
// Complexity : 9M+6A
Vector result;
Map<Vector3d>(result.data)=Map<Matrix3d>(this->data)*Map<Vector3d>(omega.data);
Map<Vector3d>(result.data)=Map<const Matrix3d>(this->data)*Map<const Vector3d>(omega.data);
return result;
}
RotationalInertia operator*(double a, const RotationalInertia& I){
RotationalInertia result;
Map<Matrix3d>(result.data)=a*Map<Matrix3d>(I.data);
Map<Matrix3d>(result.data)=a*Map<const Matrix3d>(I.data);
return result;
}
RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib){
RotationalInertia result;
Map<Matrix3d>(result.data)=Map<Matrix3d>(Ia.data)+Map<Matrix3d>(Ib.data);
Map<Matrix3d>(result.data)=Map<const Matrix3d>(Ia.data)+Map<const Matrix3d>(Ib.data);
return result;
}
}

5
src/Mod/Robot/App/kdl_cp/rotationalinertia.hpp
View File

@ -34,7 +34,7 @@ namespace KDL
class RotationalInertia{
public:
RotationalInertia(double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0);
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);
@ -65,6 +65,9 @@ namespace KDL
double data[9];
};
RotationalInertia operator*(double a, const RotationalInertia& I);
RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib);
}
#endif

15
src/Mod/Robot/App/kdl_cp/segment.hpp
View File

@ -60,9 +60,9 @@ namespace KDL {
* Joint(Joint::None)
* @param f_tip frame from the end of the joint to the tip of
* the segment, default: Frame::Identity()
* @param I rigid body inertia of the segment, default: Inertia::Zero()
* @param M rigid body inertia of the segment, default: Inertia::Zero()
*/
Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
explicit Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
/**
* Constructor of the segment
*
@ -70,9 +70,9 @@ namespace KDL {
* Joint(Joint::None)
* @param f_tip frame from the end of the joint to the tip of
* the segment, default: Frame::Identity()
* @param I rigid body inertia of the segment, default: Inertia::Zero()
* @param M rigid body inertia of the segment, default: Inertia::Zero()
*/
Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
explicit Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
Segment(const Segment& in);
Segment& operator=(const Segment& arg);
@ -144,11 +144,12 @@ namespace KDL {
* Request the pose from the joint end to the tip of the
*segment.
*
* @return const reference to the joint end - segment tip pose.
* @return the original parent end - segment end pose.
*/
const Frame& getFrameToTip()const
Frame getFrameToTip()const
{
return f_tip;
return joint.pose(0)*f_tip;
}
};

129
src/Mod/Robot/App/kdl_cp/solveri.hpp Normal file
View File

@ -0,0 +1,129 @@
// 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
};
/// 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 return "UNKNOWN ERROR";
}
protected:
/// Latest error, initialized to E_NOERROR in constructor
int error;
};
} // namespaces
#endif

6
src/Mod/Robot/App/kdl_cp/trajectory.hpp
View File

@ -78,12 +78,6 @@ namespace KDL {
class Trajectory
{
public:
virtual Path* GetPath() = 0;
// The underlying Path
virtual VelocityProfile* GetProfile() = 0;
// The velocity profile
virtual double Duration() const = 0;
// The duration of the trajectory

16
src/Mod/Robot/App/kdl_cp/trajectory_composite.cpp
View File

@ -23,7 +23,6 @@ namespace KDL {
Trajectory_Composite::Trajectory_Composite():duration(0.0)
{
path = new Path_Composite();
}
double Trajectory_Composite::Duration() const{
@ -72,7 +71,7 @@ namespace KDL {
Twist Trajectory_Composite::Acc(double time) const {
// not optimal, could be done in log(#elem)
unsigned int i;
unsigned int i;
Trajectory* traj;
double previoustime;
if (time < 0) {
@ -93,7 +92,6 @@ namespace KDL {
vt.insert(vt.end(),elem);
duration += elem->Duration();
vd.insert(vd.end(),duration);
path->Add(elem->GetPath(),false);
}
void Trajectory_Composite::Destroy() {
@ -103,8 +101,6 @@ namespace KDL {
}
vt.erase(vt.begin(),vt.end());
vd.erase(vd.begin(),vd.end());
delete path;
}
Trajectory_Composite::~Trajectory_Composite() {
@ -129,16 +125,6 @@ namespace KDL {
return comp;
}
Path* Trajectory_Composite::GetPath()
{
return path;
}
VelocityProfile* Trajectory_Composite::GetProfile()
{
return 0;
}
}

6
src/Mod/Robot/App/kdl_cp/trajectory_composite.hpp
View File

@ -37,7 +37,6 @@ class Trajectory_Composite: public Trajectory
VectorDouble vd; // contains end time for each Trajectory
double duration; // total duration of the composed
// Trajectory
Path_Composite* path;
public:
Trajectory_Composite();
@ -54,11 +53,6 @@ class Trajectory_Composite: public Trajectory
virtual void Destroy();
virtual void Write(std::ostream& os) const;
virtual Trajectory* Clone() const;
virtual Path* GetPath();
virtual VelocityProfile* GetProfile();
// access the single members
Trajectory *Get(unsigned int n){return vt[n];}
virtual ~Trajectory_Composite();
};

17
src/Mod/Robot/App/kdl_cp/trajectory_segment.cpp
View File

@ -59,15 +59,6 @@ Trajectory_Segment::Trajectory_Segment(Path* _geom, VelocityProfile* _motprof, d
motprof->SetProfileDuration(0, geom->PathLength(), _duration);
}
Path* Trajectory_Segment::GetPath()
{
return geom;
}
VelocityProfile* Trajectory_Segment::GetProfile()
{
return motprof;
}
double Trajectory_Segment::Duration() const
{
@ -106,5 +97,13 @@ Trajectory_Segment::~Trajectory_Segment()
delete geom;
}
}
Path* Trajectory_Segment::GetPath() {
return geom;
}
VelocityProfile* Trajectory_Segment::GetProfile() {
return motprof;
}
}

11
src/Mod/Robot/App/kdl_cp/trajectory_segment.hpp
View File

@ -66,18 +66,16 @@ namespace KDL {
bool aggregate;
public:
/**
* This constructor assumes that \a geom and \<_motprof\> are initialised correctly.
* 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
* 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 Path* GetPath();
virtual VelocityProfile* GetProfile();
virtual double Duration() const;
// The duration of the trajectory
@ -98,6 +96,11 @@ namespace KDL {
virtual void Write(std::ostream& os) const;
virtual Path* GetPath();
virtual VelocityProfile* GetProfile();
virtual ~Trajectory_Segment();
};

4
src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp
View File

@ -45,6 +45,10 @@ namespace KDL {
return Twist::Zero();
}
virtual void Write(std::ostream& os) const;
virtual Trajectory* Clone() const {
return new Trajectory_Stationary(duration,pos);
}
virtual ~Trajectory_Stationary() {}
};

33
src/Mod/Robot/App/kdl_cp/tree.cpp
View File

@ -26,7 +26,8 @@ namespace KDL {
using namespace std;
Tree::Tree(const std::string& _root_name) :
nrOfJoints(0),nrOfSegments(0),root_name(_root_name) {
nrOfJoints(0), nrOfSegments(0), root_name(_root_name)
{
segments.insert(make_pair(root_name, TreeElement::Root(root_name)));
}
@ -37,7 +38,7 @@ Tree::Tree(const Tree& in) {
root_name = in.root_name;
segments.insert(make_pair(root_name, TreeElement::Root(root_name)));
this->addTree(in, root_name);
addTree(in, root_name);
}
Tree& Tree::operator=(const Tree& in) {
@ -59,12 +60,18 @@ bool Tree::addSegment(const Segment& segment, const std::string& hook_name) {
pair<SegmentMap::iterator, bool> retval;
//insert new element
unsigned int q_nr = segment.getJoint().getType() != Joint::None ? nrOfJoints : 0;
retval = segments.insert(make_pair(segment.getName(), TreeElement(segment, parent, q_nr)));
#ifdef KDL_USE_NEW_TREE_INTERFACE
retval = segments.insert(make_pair(segment.getName(), TreeElementType( new TreeElement(segment, parent, q_nr))));
#else //#ifdef KDL_USE_NEW_TREE_INTERFACE
retval = segments.insert(make_pair(segment.getName(), TreeElementType(segment, parent, q_nr)));
#endif //#ifdef KDL_USE_NEW_TREE_INTERFACE
//check if insertion succeeded
if (!retval.second)
return false;
//add iterator to new element in parents children list
parent->second.children.push_back(retval.first);
GetTreeElementChildren(parent->second).push_back(retval.first);
//increase number of segments
nrOfSegments++;
//increase number of joints
@ -92,10 +99,10 @@ bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string&
//get iterator for root-segment
SegmentMap::const_iterator child;
//try to add all of root's children
for (unsigned int i = 0; i < root->second.children.size(); i++) {
child = root->second.children[i];
for (unsigned int i = 0; i < GetTreeElementChildren(root->second).size(); i++) {
child = GetTreeElementChildren(root->second)[i];
//Try to add the child
if (this->addSegment(child->second.segment, hook_name)) {
if (this->addSegment(GetTreeElementSegment(child->second), hook_name)) {
//if child is added, add all the child's children
if (!(this->addTreeRecursive(child, child->first)))
//if it didn't work, return false
@ -114,12 +121,12 @@ bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string&
// walk down from chain_root and chain_tip to the root of the tree
vector<SegmentMap::key_type> parents_chain_root, parents_chain_tip;
for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s=s->second.parent){
for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s = GetTreeElementParent(s->second)){
parents_chain_root.push_back(s->first);
if (s->first == root_name) break;
}
if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false;
for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s=s->second.parent){
for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s = GetTreeElementParent(s->second)){
parents_chain_tip.push_back(s->first);
if (s->first == root_name) break;
}
@ -138,20 +145,20 @@ bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string&
// add the segments from the root to the common frame
for (unsigned int s=0; s<parents_chain_root.size()-1; s++){
Segment seg = getSegment(parents_chain_root[s])->second.segment;
Segment seg = GetTreeElementSegment(getSegment(parents_chain_root[s])->second);
Frame f_tip = seg.pose(0.0).Inverse();
Joint jnt = seg.getJoint();
if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis)
jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::RotAxis);
else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis)
jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::TransAxis);
chain.addSegment(Segment(getSegment(parents_chain_root[s+1])->second.segment.getName(),
jnt, f_tip, getSegment(parents_chain_root[s+1])->second.segment.getInertia()));
chain.addSegment(Segment(GetTreeElementSegment(getSegment(parents_chain_root[s+1])->second).getName(),
jnt, f_tip, GetTreeElementSegment(getSegment(parents_chain_root[s+1])->second).getInertia()));
}
// add the segments from the common frame to the tip frame
for (int s=parents_chain_tip.size()-1; s>-1; s--){
chain.addSegment(getSegment(parents_chain_tip[s])->second.segment);
chain.addSegment(GetTreeElementSegment(getSegment(parents_chain_tip[s])->second));
}
return true;
}

73
src/Mod/Robot/App/kdl_cp/tree.hpp
View File

@ -22,38 +22,72 @@
#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
{
//Forward declaration
class TreeElement;
#ifdef KDL_USE_NEW_TREE_INTERFACE
//We use smart pointers for managing tree nodes for now becuase
//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
{
private:
TreeElement(const std::string& name):segment(name), q_nr(0)
{};
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;
TreeElement(const Segment& segment_in,const SegmentMap::const_iterator& parent_in,unsigned int q_nr_in)
{
q_nr=q_nr_in;
segment=segment_in;
parent=parent_in;
};
static TreeElement Root(const std::string& root_name)
{
return TreeElement(root_name);
};
private:
TreeElement(const std::string& name):segment(name), q_nr(0) {}
};
/**
@ -66,8 +100,8 @@ namespace KDL
{
private:
SegmentMap segments;
int nrOfJoints;
int nrOfSegments;
unsigned int nrOfJoints;
unsigned int nrOfSegments;
std::string root_name;
@ -77,7 +111,7 @@ namespace KDL
/**
* The constructor of a tree, a new tree is always empty
*/
Tree(const std::string& root_name="root");
explicit Tree(const std::string& root_name="root");
Tree(const Tree& in);
Tree& operator= (const Tree& arg);
@ -97,7 +131,6 @@ namespace KDL
* Adds a complete chain to the end of the segment with
* hook_name as segment_name.
*
* @param chain the chain.
* @param hook_name name of the segment to connect the chain with.
*
* @return false if hook_name could not be found.
@ -156,7 +189,9 @@ namespace KDL
};
/**
* Request the chain of the tree between chain_root and chain_tip. The chain_root must be an ancester from chain_tip
* 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

1
src/Mod/Robot/App/kdl_cp/treefksolver.hpp
View File

@ -50,7 +50,6 @@ namespace KDL {
*
* @param q_in input joint coordinates
* @param p_out reference to output cartesian pose
* @param segmentName
*
* @return if < 0 something went wrong
*/

12
src/Mod/Robot/App/kdl_cp/treefksolverpos_recursive.cpp
View File

@ -40,7 +40,7 @@ namespace KDL {
else if(it == tree.getSegments().end()) //if the segment name is not found
return -2;
else{
p_out = recursiveFk(q_in, it);
p_out = recursiveFk(q_in, it);
return 0;
}
}
@ -48,15 +48,15 @@ namespace KDL {
Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it)
{
//gets the frame for the current element (segment)
const TreeElement& currentElement = it->second;
Frame currentFrame = currentElement.segment.pose(q_in(currentElement.q_nr));
SegmentMap::const_iterator rootIterator = tree.getSegment("root");
const TreeElementType& currentElement = it->second;
Frame currentFrame = GetTreeElementSegment(currentElement).pose(q_in(GetTreeElementQNr(currentElement)));
SegmentMap::const_iterator rootIterator = tree.getRootSegment();
if(it == rootIterator){
return currentFrame;
}
else{
SegmentMap::const_iterator parentIt = currentElement.parent;
SegmentMap::const_iterator parentIt = GetTreeElementParent(currentElement);
return recursiveFk(q_in, parentIt) * currentFrame;
}
}

1
src/Mod/Robot/App/kdl_cp/treeiksolver.hpp
View File

@ -11,6 +11,7 @@
#include "tree.hpp"
#include "jntarray.hpp"
#include "frames.hpp"
#include <map>
namespace KDL {

11
src/Mod/Robot/App/kdl_cp/treeiksolverpos_nr_jl.cpp
View File

@ -29,13 +29,10 @@ namespace KDL {
const JntArray& _q_min, const JntArray& _q_max,
TreeFkSolverPos& _fksolver, TreeIkSolverVel& _iksolver,
unsigned int _maxiter, double _eps) :
tree(_tree), q_min(tree.getNrOfJoints()), q_max(tree.getNrOfJoints()),
iksolver(_iksolver), fksolver(_fksolver), delta_q(tree.getNrOfJoints()),
endpoints(_endpoints), maxiter(_maxiter), eps(_eps) {
q_min = _q_min;
q_max = _q_max;
tree(_tree), q_min(_q_min), q_max(_q_max), iksolver(_iksolver),
fksolver(_fksolver), delta_q(tree.getNrOfJoints()),
endpoints(_endpoints), maxiter(_maxiter), eps(_eps)
{
for (size_t i = 0; i < endpoints.size(); i++) {
frames.insert(Frames::value_type(endpoints[i], Frame::Identity()));
delta_twists.insert(Twists::value_type(endpoints[i], Twist::Zero()));

175
src/Mod/Robot/App/kdl_cp/treeiksolverpos_online.cpp Normal file
View File

@ -0,0 +1,175 @@
// 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
#include "treeiksolverpos_online.hpp"
namespace KDL {
TreeIkSolverPos_Online::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) :
q_min_(nr_of_jnts),
q_max_(nr_of_jnts),
q_dot_max_(nr_of_jnts),
fksolver_(fksolver),
iksolver_(iksolver),
q_dot_(nr_of_jnts)
{
q_min_ = q_min;
q_max_ = q_max;
q_dot_max_ = q_dot_max;
x_dot_trans_max_ = x_dot_trans_max;
x_dot_rot_max_ = x_dot_rot_max;
for (size_t i = 0; i < endpoints.size(); i++)
{
frames_.insert(Frames::value_type(endpoints[i], Frame::Identity()));
delta_twists_.insert(Twists::value_type(endpoints[i], Twist::Zero()));
}
}
TreeIkSolverPos_Online::~TreeIkSolverPos_Online()
{}
double TreeIkSolverPos_Online::CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
{
q_out = q_in;
// First check, if all elements in p_in are available
for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
if(frames_.find(f_des_it->first)==frames_.end())
return -2;
for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
{
// Get all iterators for this endpoint
Frames::iterator f_it = frames_.find(f_des_it->first);
Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);
fksolver_.JntToCart(q_out, f_it->second, f_it->first);
twist_ = diff(f_it->second, f_des_it->second);
// Checks, if the twist (twist_) exceeds the maximum translational and/or rotational velocity
// And scales them, if necessary
enforceCartVelLimits();
delta_twists_it->second = twist_;
}
double res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);
if(res<0)
return res;
//If we got here q_out is definitely of the right size
if(q_out.rows()!=q_min_.rows() || q_out.rows()!=q_max_.rows() || q_out.rows()!= q_dot_max_.rows())
return -1;
// Checks, if joint velocities (q_dot_) exceed their maximum and scales them, if necessary
enforceJointVelLimits();
// Integrate
Add(q_out, q_dot_, q_out);
// Limit joint positions
for (unsigned int j = 0; j < q_min_.rows(); j++)
{
if (q_out(j) < q_min_(j))
q_out(j) = q_min_(j);
else if (q_out(j) > q_max_(j))
q_out(j) = q_max_(j);
}
return res;
}
void TreeIkSolverPos_Online::enforceJointVelLimits()
{
// check, if one (or more) joint velocities exceed the maximum value
// and if so, safe the biggest overshoot for scaling q_dot_ properly
// to keep the direction of the velocity vector the same
double rel_os, rel_os_max = 0.0; // relative overshoot and the biggest relative overshoot
bool max_exceeded = false;
for (unsigned int i = 0; i < q_dot_.rows(); i++)
{
if ( q_dot_(i) > q_dot_max_(i) )
{
max_exceeded = true;
rel_os = (q_dot_(i) - q_dot_max_(i)) / q_dot_max_(i);
if ( rel_os > rel_os_max )
{
rel_os_max = rel_os;
}
}
else if ( q_dot_(i) < -q_dot_max_(i) )
{
max_exceeded = true;
rel_os = (-q_dot_(i) - q_dot_max_(i)) / q_dot_max_(i);
if ( rel_os > rel_os_max)
{
rel_os_max = rel_os;
}
}
}
// scales q_out, if one joint exceeds the maximum value
if ( max_exceeded == true )
{
Multiply(q_dot_, ( 1.0 / ( 1.0 + rel_os_max ) ), q_dot_);
}
}
void TreeIkSolverPos_Online::enforceCartVelLimits()
{
double x_dot_trans, x_dot_rot; // vector lengths
x_dot_trans = sqrt( pow(twist_.vel.x(), 2) + pow(twist_.vel.y(), 2) + pow(twist_.vel.z(), 2));
x_dot_rot = sqrt( pow(twist_.rot.x(), 2) + pow(twist_.rot.y(), 2) + pow(twist_.rot.z(), 2));
if ( x_dot_trans > x_dot_trans_max_ || x_dot_rot > x_dot_rot_max_ )
{
if ( x_dot_trans > x_dot_rot )
{
twist_.vel = twist_.vel * ( x_dot_trans_max_ / x_dot_trans );
twist_.rot = twist_.rot * ( x_dot_trans_max_ / x_dot_trans );
}
else if ( x_dot_rot > x_dot_trans )
{
twist_.vel = twist_.vel * ( x_dot_rot_max_ / x_dot_rot );
twist_.rot = twist_.rot * ( x_dot_rot_max_ / x_dot_rot );
}
}
}
} // namespace

108
src/Mod/Robot/App/kdl_cp/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 propotional 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 propotional 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 */

23
src/Mod/Robot/App/kdl_cp/treeiksolvervel_wdls.cpp
View File

@ -22,7 +22,8 @@ namespace KDL {
Wy_U(J.rows(),J.rows()),Wq_V(J.cols(),J.cols()),
t(VectorXd::Zero(J.rows())), Wy_t(VectorXd::Zero(J.rows())),
qdot(VectorXd::Zero(J.cols())),
tmp(VectorXd::Zero(J.cols())),S(VectorXd::Zero(J.cols()))
tmp(VectorXd::Zero(J.cols())),S(VectorXd::Zero(J.cols())),
lambda(0)
{
for (size_t i = 0; i < endpoints.size(); ++i) {
@ -68,30 +69,30 @@ namespace KDL {
//lets put the jacobian in the big matrix and put the twist in the big t:
J.block(6*k,0, 6,tree.getNrOfJoints()) = jac_it->second.data;
const Twist& twist=v_in.find(jac_it->first)->second;
t.segment(6*k,3) = Eigen::Map<Eigen::Vector3d>(twist.vel.data);
t.segment(6*k+3,3) = Eigen::Map<Eigen::Vector3d>(twist.rot.data);
t.segment(6*k,3) = Eigen::Map<const Eigen::Vector3d>(twist.vel.data);
t.segment(6*k+3,3) = Eigen::Map<const Eigen::Vector3d>(twist.rot.data);
}
++k;
}
//Lets use the wdls algorithm to find the qdot:
// Create the Weighted jacobian
J_Wq = (J * Wq).lazy();
Wy_J_Wq = (Wy * J_Wq).lazy();
J_Wq.noalias() = J * Wq;
Wy_J_Wq.noalias() = Wy * J_Wq;
// Compute the SVD of the weighted jacobian
int ret = svd_eigen_HH(Wy_J_Wq, U, S, V, tmp);
(void)ret;
if (ret < 0 )
return E_SVD_FAILED;
//Pre-multiply U and V by the task space and joint space weighting matrix respectively
Wy_t = (Wy * t).lazy();
Wq_V = (Wq * V).lazy();
Wy_t.noalias() = Wy * t;
Wq_V.noalias() = Wq * V;
// tmp = (Si*Wy*U'*y),
for (unsigned int i = 0; i < J.cols(); i++) {
double sum = 0.0;
for (unsigned int j = 0; j < J.rows(); j++) {
if (i < Wy_t.size())
if (i < Wy_t.rows())
sum += U(j, i) * Wy_t(j);
else
sum += 0.0;
@ -100,7 +101,7 @@ namespace KDL {
}
// x = Lx^-1*V*tmp + x
qdot_out.data = (Wq_V * tmp).lazy();
qdot_out.data.noalias() = Wq_V * tmp;
return Wy_t.norm();
}

4
src/Mod/Robot/App/kdl_cp/treeiksolvervel_wdls.hpp
View File

@ -14,10 +14,12 @@
namespace KDL {
USING_PART_OF_NAMESPACE_EIGEN;
using namespace Eigen;
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();

12
src/Mod/Robot/App/kdl_cp/treejnttojacsolver.cpp
View File

@ -33,22 +33,22 @@ int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, const std:
//Let's make the jacobian zero:
SetToZero(jac);
SegmentMap::const_iterator root = tree.getSegments().find("root");
SegmentMap::const_iterator root = tree.getRootSegment();
Frame T_total = Frame::Identity();
//Lets recursively iterate until we are in the root segment
while (it != root) {
//get the corresponding q_nr for this TreeElement:
unsigned int q_nr = it->second.q_nr;
unsigned int q_nr = GetTreeElementQNr(it->second);
//get the pose of the segment:
Frame T_local = it->second.segment.pose(q_in(q_nr));
Frame T_local = GetTreeElementSegment(it->second).pose(q_in(q_nr));
//calculate new T_end:
T_total = T_local * T_total;
//get the twist of the segment:
if (it->second.segment.getJoint().getType() != Joint::None) {
Twist t_local = it->second.segment.twist(q_in(q_nr), 1.0);
if (GetTreeElementSegment(it->second).getJoint().getType() != Joint::None) {
Twist t_local = GetTreeElementSegment(it->second).twist(q_in(q_nr), 1.0);
//transform the endpoint of the local twist to the global endpoint:
t_local = t_local.RefPoint(T_total.p - T_local.p);
//transform the base of the twist to the endpoint
@ -57,7 +57,7 @@ int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, const std:
jac.setColumn(q_nr,t_local);
}//endif
//goto the parent
it = it->second.parent;
it = GetTreeElementParent(it->second);
}//endwhile
//Change the base of the complete jacobian from the endpoint to the base
changeBase(jac, T_total.M, jac);

2
src/Mod/Robot/App/kdl_cp/treejnttojacsolver.hpp
View File

@ -16,7 +16,7 @@ namespace KDL {
class TreeJntToJacSolver {
public:
TreeJntToJacSolver(const Tree& tree);
explicit TreeJntToJacSolver(const Tree& tree);
virtual ~TreeJntToJacSolver();

10
src/Mod/Robot/App/kdl_cp/utilities/error.h
View File

@ -46,8 +46,8 @@
#define ERROR_H_84822
#include "utility.h"
#include <string.h>
#include <string>
namespace KDL {
/**
@ -180,9 +180,13 @@ public:
};
class Error_MotionPlanning_Not_Feasible: public Error_MotionPlanning {
int reason;
public:
virtual const char* Description() const { return "Motion Profile with requested parameters is not feasible";}
virtual int GetType() const {return 3004;}
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 {

2
src/Mod/Robot/App/kdl_cp/utilities/error_stack.cxx
View File

@ -26,8 +26,8 @@ namespace KDL {
// interprete error messages from I/O
typedef std::stack<std::string> ErrorStack;
ErrorStack errorstack;
// should be in Thread Local Storage if this gets multithreaded one day...
static ErrorStack errorstack;
void IOTrace(const std::string& description) {

9
src/Mod/Robot/App/kdl_cp/utilities/rallNd.h
View File

@ -10,8 +10,7 @@
* 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 >
@ -46,8 +45,8 @@ public:
* 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
* ==> 1 2 3 4
* ==> 3 4 5 6
* 4 5 6 7
*
* de aangeduide berekeningen zijn niet noodzakelijk, en er is dan niets
@ -76,7 +75,7 @@ public:
template <>
class RallNd<2> : public Rall2d<double> {
public:
RallNd() {} /* (dwz. met evenveel numerieke operaties als een */
RallNd() {}* (dwz. met evenveel numerieke operaties als een
RallNd(const Rall2d<double>& arg) :
Rall2d<double>(arg) {}
RallNd(double value,double d[]) {

25
src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.cpp
View File

@ -30,7 +30,7 @@ namespace KDL{
const int cols = A.cols();
U.setZero();
U.corner(Eigen::TopLeft,rows,cols)=A;
U.topLeftCorner(rows,cols)=A;
int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
int ppi(0);
@ -53,14 +53,14 @@ namespace KDL{
s += U(k,i)*U(k,i);
}
f=U(i,i); // f is the diag elem
assert(s>=0);
if (!(s>=0)) return -3;
g = -SIGN(sqrt(s),f);
h=f*g-s;
U(i,i)=f-g;
for (j=ppi;j<cols;j++) {
// dot product of columns i and j, starting from the i-th row
for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
assert(h!=0);
if (!(h!=0)) return -4;
f=s/h;
// copy the scaled i-th column into the j-th column
for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
@ -80,11 +80,11 @@ namespace KDL{
s += U(i,k)*U(i,k);
}
f=U(i,ppi);
assert(s>=0);
if (!(s>=0)) return -5;
g = -SIGN(sqrt(s),f);
h=f*g-s;
U(i,ppi)=f-g;
assert(h!=0);
if (!(h!=0)) return -6;
for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
for (j=ppi;j<rows;j++) {
for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
@ -101,7 +101,7 @@ namespace KDL{
for (i=cols-1;i>=0;i--) {
if (i<cols-1) {
if (fabs(g)>epsilon) {
assert(U(i,ppi)!=0);
if (!(U(i,ppi)!=0)) return -7;
for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
for (j=ppi;j<cols;j++) {
for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
@ -123,7 +123,7 @@ namespace KDL{
g=1.0/g;
for (j=ppi;j<cols;j++) {
for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
assert(U(i,i)!=0);
if (!(U(i,i)!=0)) return -8;
f=(s/U(i,i))*g;
for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
}
@ -156,7 +156,7 @@ namespace KDL{
g=S(i);
h=PYTHAG(f,g);
S(i)=h;
assert(h!=0);
if (!(h!=0)) return -9;
h=1.0/h;
c=g*h;
s=(-f*h);
@ -183,12 +183,12 @@ namespace KDL{
y=S(nm);
g=tmp(nm);
h=tmp(k);
assert(h!=0&&y!=0);
if (!(h!=0&&y!=0)) return -10;
f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
g=PYTHAG(f,1.0);
assert(x!=0);
assert((f+SIGN(g,f))!=0);
if (!(x!=0)) return -11;
if (!((f+SIGN(g,f))!=0)) return -12;
f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
/* Next QR transformation: */
@ -201,7 +201,7 @@ namespace KDL{
g=c*g;
z=PYTHAG(f,h);
tmp(j)=z;
assert(z!=0);
if (!(z!=0)) return -13;
c=f/z;
s=h/z;
f=x*c+g*s;
@ -216,7 +216,6 @@ namespace KDL{
}
z=PYTHAG(f,h);
S(j)=z;
assert(z!=0);
if (fabs(z)>epsilon) {
z=1.0/z;
c=f*z;

3
src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.hpp
View File

@ -28,7 +28,7 @@
#include <Eigen/Core>
#include <algorithm>
USING_PART_OF_NAMESPACE_EIGEN;
using namespace Eigen;
namespace KDL
{
@ -63,7 +63,6 @@ namespace KDL
* @param V matrix<double>(nxn)
* @param tmp vector<double> n
* @param maxiter defaults to 150
* @param epsilon defaults to 1e-300
*
* @return -2 if maxiter exceeded, 0 otherwise
*/

44
src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_Macie.hpp
View File

@ -22,14 +22,40 @@
//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_BOOST_MACIE
#define SVD_BOOST_MACIE
#include <Eigen/Core>
USING_PART_OF_NAMESPACE_EIGEN
using namespace Eigen;
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 thresshold [INPUT] Thresshold 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 MatrixXd& A,MatrixXd& U,VectorXd& S, MatrixXd& V,
MatrixXd& B, VectorXd& tempi,
double treshold,bool toggle)
@ -39,7 +65,7 @@ namespace KDL
unsigned int rotations=0;
if(toggle){
//Calculate B from new A and previous V
B=(A*V).lazy();
B=A.lazyProduct(V);
while(rotate){
rotate=false;
rotations=0;
@ -76,9 +102,9 @@ namespace KDL
B.col(i) = tempi;
//Apply plane rotation to columns of V
tempi.start(V.rows()) = cos*V.col(i) + sin*V.col(j);
tempi.head(V.rows()) = cos*V.col(i) + sin*V.col(j);
V.col(j) = - sin*V.col(i) + cos*V.col(j);
V.col(i) = tempi.start(V.rows());
V.col(i) = tempi.head(V.rows());
rotate=true;
}
@ -103,7 +129,7 @@ namespace KDL
return sweeps;
}else{
//Calculate B from new A and previous U'
B =(U.transpose() * A).lazy();
B = U.transpose().lazyProduct(A);
while(rotate){
rotate=false;
rotations=0;
@ -137,15 +163,15 @@ namespace KDL
}
//Apply plane rotation to rows of B
tempi.start(B.cols()) = cos*B.row(i) + sin*B.row(j);
tempi.head(B.cols()) = cos*B.row(i) + sin*B.row(j);
B.row(j) = - sin*B.row(i) + cos*B.row(j);
B.row(i) = tempi.start(B.cols());
B.row(i) = tempi.head(B.cols());
//Apply plane rotation to rows of U
tempi.start(U.rows()) = cos*U.col(i) + sin*U.col(j);
tempi.head(U.rows()) = cos*U.col(i) + sin*U.col(j);
U.col(j) = - sin*U.col(i) + cos*U.col(j);
U.col(i) = tempi.start(U.rows());
U.col(i) = tempi.head(U.rows());
rotate=true;
}

2
src/Mod/Robot/App/kdl_cp/utilities/utility.cxx
View File

@ -1,4 +1,4 @@
/** @file utility.cxx
/** @file utility.cpp
* @author Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
* @version
* ORO_Geometry V0.2

194
src/Mod/Robot/App/kdl_cp/velocityprofile_spline.cpp Normal file
View File

@ -0,0 +1,194 @@
#include <limits>
#include "velocityprofile_spline.hpp"
namespace KDL {
static inline void generatePowers(int n, double x, double* powers)
{
powers[0] = 1.0;
for (int i=1; i<=n; i++)
{
powers[i] = powers[i-1]*x;
}
return;
}
VelocityProfile_Spline::VelocityProfile_Spline()
{
duration_ = 0.0;
coeff_[0] = 0.0;
coeff_[1] = 0.0;
coeff_[2] = 0.0;
coeff_[3] = 0.0;
coeff_[4] = 0.0;
coeff_[5] = 0.0;
return;
}
VelocityProfile_Spline::VelocityProfile_Spline(const VelocityProfile_Spline &p)
{
duration_ = p.duration_;
coeff_[0] = p.coeff_[0];
coeff_[1] = p.coeff_[1];
coeff_[2] = p.coeff_[2];
coeff_[3] = p.coeff_[3];
coeff_[4] = p.coeff_[4];
coeff_[5] = p.coeff_[5];
return;
}
VelocityProfile_Spline::~VelocityProfile_Spline()
{
return;
}
void VelocityProfile_Spline::SetProfile(double pos1, double pos2)
{
return;
}
void VelocityProfile_Spline::SetProfileDuration(double pos1, double pos2, double duration)
{
duration_ = duration;
if (duration <= std::numeric_limits<double>::epsilon() )
{
coeff_[0] = pos1;
coeff_[1] = 0.0;
coeff_[2] = 0.0;
coeff_[3] = 0.0;
coeff_[4] = 0.0;
coeff_[5] = 0.0;
} else
{
coeff_[0] = pos1;
coeff_[1] = (pos2 - pos1) / duration;
coeff_[2] = 0.0;
coeff_[3] = 0.0;
coeff_[4] = 0.0;
coeff_[5] = 0.0;
}
return;
}
void VelocityProfile_Spline::SetProfileDuration(double pos1, double vel1, double pos2, double vel2, double duration)
{
double T[4];
duration_ = duration;
generatePowers(3, duration, T);
if (duration <= std::numeric_limits<double>::epsilon() )
{
coeff_[0] = pos2;
coeff_[1] = vel2;
coeff_[2] = 0.0;
coeff_[3] = 0.0;
coeff_[4] = 0.0;
coeff_[5] = 0.0;
} else
{
coeff_[0] = pos1;
coeff_[1] = vel1;
coeff_[2] = (-3.0*pos1 + 3.0*pos2 - 2.0*vel1*T[1] - vel2*T[1]) / T[2];
coeff_[3] = (2.0*pos1 - 2.0*pos2 + vel1*T[1] + vel2*T[1]) / T[3];
coeff_[4] = 0.0;
coeff_[5] = 0.0;
}
return;
}
void VelocityProfile_Spline::SetProfileDuration(double pos1, double vel1, double acc1, double pos2, double vel2, double acc2, double duration)
{
double T[6];
duration_ = duration;
generatePowers(5, duration, T);
if (duration <= std::numeric_limits<double>::epsilon() )
{
coeff_[0] = pos2;
coeff_[1] = vel2;
coeff_[2] = 0.5 * acc2;
coeff_[3] = 0.0;
coeff_[4] = 0.0;
coeff_[5] = 0.0;
} else
{
coeff_[0] = pos1;
coeff_[1] = vel1;
coeff_[2] = 0.5*acc1;
coeff_[3] = (-20.0*pos1 + 20.0*pos2 - 3.0*acc1*T[2] + acc2*T[2] -
12.0*vel1*T[1] - 8.0*vel2*T[1]) / (2.0*T[3]);
coeff_[4] = (30.0*pos1 - 30.0*pos2 + 3.0*acc1*T[2] - 2.0*acc2*T[2] +
16.0*vel1*T[1] + 14.0*vel2*T[1]) / (2.0*T[4]);
coeff_[5] = (-12.0*pos1 + 12.0*pos2 - acc1*T[2] + acc2*T[2] -
6.0*vel1*T[1] - 6.0*vel2*T[1]) / (2.0*T[5]);
}
return;
}
double VelocityProfile_Spline::Duration() const
{
return duration_;
}
double VelocityProfile_Spline::Pos(double time) const
{
double t[6];
double position;
generatePowers(5, time, t);
position = t[0]*coeff_[0] +
t[1]*coeff_[1] +
t[2]*coeff_[2] +
t[3]*coeff_[3] +
t[4]*coeff_[4] +
t[5]*coeff_[5];
return position;
}
double VelocityProfile_Spline::Vel(double time) const
{
double t[5];
double velocity;
generatePowers(4, time, t);
velocity = t[0]*coeff_[1] +
2.0*t[1]*coeff_[2] +
3.0*t[2]*coeff_[3] +
4.0*t[3]*coeff_[4] +
5.0*t[4]*coeff_[5];
return velocity;
}
double VelocityProfile_Spline::Acc(double time) const
{
double t[4];
double acceleration;
generatePowers(3, time, t);
acceleration = 2.0*t[0]*coeff_[2] +
6.0*t[1]*coeff_[3] +
12.0*t[2]*coeff_[4] +
20.0*t[3]*coeff_[5];
return acceleration;
}
void VelocityProfile_Spline::Write(std::ostream& os) const
{
os << "coefficients : [ " << coeff_[0] << " " << coeff_[1] << " " << coeff_[2] << " " << coeff_[3] << " " << coeff_[4] << " " << coeff_[5] << " ]";
return;
}
VelocityProfile* VelocityProfile_Spline::Clone() const
{
return new VelocityProfile_Spline(*this);
}
}

67
src/Mod/Robot/App/kdl_cp/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 coeffcients.
*
* @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 coeffcients.
*
* @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 coeffcients.
*
* @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

19
src/Mod/Robot/App/kdl_cp/velocityprofile_trap.cpp
View File

@ -108,6 +108,25 @@ void VelocityProfile_Trap::SetProfileDuration(
t2 /= factor;
}
void VelocityProfile_Trap::SetProfileVelocity(
double pos1,double pos2,double newvelocity) {
// Max velocity
SetProfile(pos1,pos2);
// Must be Slower :
double factor = newvelocity; // valid = [KDL::epsilon, 1.0]
if (1.0 < factor) factor = 1.0;
if (KDL::epsilon > factor) factor = KDL::epsilon;
a2*=factor;
a3*=factor*factor;
b2*=factor;
b3*=factor*factor;
c2*=factor;
c3*=factor*factor;
duration = duration / factor;
t1 /= factor;
t2 /= factor;
}
void VelocityProfile_Trap::SetMax(double _maxvel,double _maxacc)
{
maxvel = _maxvel; maxacc = _maxacc;

Some files were not shown because too many files have changed in this diff Show More