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

提交依赖与构建产物

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

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

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

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

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

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

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

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

@@ -0,0 +1,33 @@
/* Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */
/* Version: 1.0 */
/* Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */
/* Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> */
/* URL: http://www.orocos.org/kdl */
/* This library is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU Lesser General Public */
/* License as published by the Free Software Foundation; either */
/* version 2.1 of the License, or (at your option) any later version. */
/* This library is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU */
/* Lesser General Public License for more details. */
/* You should have received a copy of the GNU Lesser General Public */
/* License along with this library; if not, write to the Free Software */
/* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA */
/* Methods are inlined */
#define KDL_INLINE 1
/* Column width that is used form printing frames */
#define KDL_FRAME_WIDTH 12
/* Indices are checked when accessing members of the objects */
#define KDL_INDEX_CHECK 1
/* use KDL implementation for == operator */
#define KDL_USE_EQUAL 1

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,78 @@
/***************************************************************************
scoped_ptr - a not too smart pointer for exception safe heap objects
-------------------------
begin : May 2019
copyright : (C) 2019 Intermodalics
***************************************************************************
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Lesser General Public *
* License as published by the Free Software Foundation; either *
* version 2.1 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
* Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public *
* License along with this library; if not, write to the Free Software *
* Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307 USA *
* *
***************************************************************************/
#if (__cplusplus > 199711L)
#include <utility>
#else
#include <algorithm>
#endif
namespace KDL {
template<typename T> class scoped_ptr;
template<typename T> void swap(scoped_ptr<T>&, scoped_ptr<T>&);
template<typename T>
class scoped_ptr {
public:
scoped_ptr() : ptr_(0) { }
explicit scoped_ptr(T* p) : ptr_(p) { }
~scoped_ptr() { delete ptr_; }
T* operator->() { return ptr_; }
const T* operator->() const { return ptr_; }
T* get() const { return ptr_; }
void reset(T* p = 0) {
T* old = ptr_;
ptr_ = p;
delete old;
}
T* release() {
T* old = ptr_;
ptr_ = 0;
return old;
}
friend void swap<>(scoped_ptr<T>& a, scoped_ptr<T>& b);
private:
scoped_ptr(const scoped_ptr&); // not-copyable
scoped_ptr& operator=(const scoped_ptr&); // not-copyable
T* ptr_;
};
template<typename T>
void swap(scoped_ptr<T>& a, scoped_ptr<T>& b) {
using std::swap;
swap(a.ptr_, b.ptr_);
}
} // namespace KDL

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

@@ -0,0 +1,46 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#ifndef KDL_SVD_HH_HPP
#define KDL_SVD_HH_HPP
#include "../jacobian.hpp"
#include "../jntarray.hpp"
#include <vector>
namespace KDL
{
class SVD_HH
{
public:
SVD_HH(const Jacobian& jac);
~SVD_HH();
int calculate(const Jacobian& jac,std::vector<JntArray>& U,
JntArray& w,std::vector<JntArray>& v,int maxiter);
private:
JntArray tmp;
};
}
#endif

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

@@ -0,0 +1,69 @@
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
//Based on the svd of the KDL-0.2 library by Erwin Aertbelien
#ifndef SVD_EIGEN_HH_HPP
#define SVD_EIGEN_HH_HPP
#include <Eigen/Core>
#include <algorithm>
namespace KDL
{
inline double PYTHAG(double a,double b) {
double at,bt,ct;
at = fabs(a);
bt = fabs(b);
if (at > bt ) {
ct=bt/at;
return at*sqrt(1.0+ct*ct);
} else {
if (bt==0)
return 0.0;
else {
ct=at/bt;
return bt*sqrt(1.0+ct*ct);
}
}
}
inline double SIGN(double a,double b) {
return ((b) >= 0.0 ? fabs(a) : -fabs(a));
}
/**
* svd calculation of eigen matrices
*
* @param A matrix<double>(mxn)
* @param U matrix<double>(mxn)
* @param S vector<double> n
* @param V matrix<double>(nxn)
* @param tmp vector<double> n
* @param maxiter defaults to 150
*
* @return -2 if maxiter exceeded, 0 otherwise
*/
int svd_eigen_HH(const Eigen::MatrixXd& A,Eigen::MatrixXd& U,Eigen::VectorXd& S,Eigen::MatrixXd& V,Eigen::VectorXd& tmp,int maxiter=150,double epsilon=1e-300);
}
#endif

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

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

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

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

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

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

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

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