diff options
Diffstat (limited to 'intern/itasc/kdl')
55 files changed, 10594 insertions, 0 deletions
diff --git a/intern/itasc/kdl/Makefile b/intern/itasc/kdl/Makefile new file mode 100644 index 00000000000..058f93da4e1 --- /dev/null +++ b/intern/itasc/kdl/Makefile @@ -0,0 +1,43 @@ +# +# $Id$ +# +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software Foundation, +# Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# +# The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. +# All rights reserved. +# +# The Original Code is: all of this file. +# +# Contributor(s): Hans Lambermont +# +# ***** END GPL LICENSE BLOCK ***** +# iksolver main makefile. +# + +include nan_definitions.mk + +LIBNAME = itasc_kdl +# Yep, same dir than parent (itasc instead of $(LIBNAME)) +DIR = $(OCGDIR)/intern/itasc +DIRS = utilities +SOURCEDIR = intern/$(LIBNAME)/kdl + +include nan_subdirs.mk +include nan_compile.mk + +CPPFLAGS += -I. +CPPFLAGS += -I../../../extern/Eigen2 diff --git a/intern/itasc/kdl/chain.cpp b/intern/itasc/kdl/chain.cpp new file mode 100644 index 00000000000..638366c96be --- /dev/null +++ b/intern/itasc/kdl/chain.cpp @@ -0,0 +1,75 @@ +// 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 + +#include "chain.hpp" + +namespace KDL { + using namespace std; + + Chain::Chain(): + segments(0), + nrOfJoints(0), + nrOfSegments(0) + { + } + + Chain::Chain(const Chain& in):nrOfJoints(0), + nrOfSegments(0) + { + for(unsigned int i=0;i<in.getNrOfSegments();i++) + this->addSegment(in.getSegment(i)); + } + + Chain& Chain::operator=(const Chain& arg) + { + nrOfJoints=0; + nrOfSegments=0; + segments.resize(0); + for(unsigned int i=0;i<arg.nrOfSegments;i++) + addSegment(arg.getSegment(i)); + return *this; + + } + + void Chain::addSegment(const Segment& segment) + { + segments.push_back(segment); + nrOfSegments++; + nrOfJoints += segment.getJoint().getNDof(); + } + + void Chain::addChain(const Chain& chain) + { + for(unsigned int i=0;i<chain.getNrOfSegments();i++) + this->addSegment(chain.getSegment(i)); + } + + const Segment& Chain::getSegment(unsigned int nr) const + { + return segments[nr]; + } + + Chain::~Chain() + { + } + +} + diff --git a/intern/itasc/kdl/chain.hpp b/intern/itasc/kdl/chain.hpp new file mode 100644 index 00000000000..0d40690202a --- /dev/null +++ b/intern/itasc/kdl/chain.hpp @@ -0,0 +1,95 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAIN_HPP +#define KDL_CHAIN_HPP + +#include "segment.hpp" +#include <string> + +namespace KDL { + /** + * \brief This class encapsulates a <strong>serial</strong> kinematic + * interconnection structure. It is build out of segments. + * + * @ingroup KinematicFamily + */ + class Chain { + private: + std::vector<Segment> segments; + unsigned int nrOfJoints; + unsigned int nrOfSegments; + public: + /** + * The constructor of a chain, a new chain is always empty. + * + */ + Chain(); + Chain(const Chain& in); + Chain& operator = (const Chain& arg); + + /** + * Adds a new segment to the <strong>end</strong> of the chain. + * + * @param segment The segment to add + */ + void addSegment(const Segment& segment); + /** + * Adds a complete chain to the <strong>end</strong> of the chain + * The added chain is copied. + * + * @param chain The chain to add + */ + void addChain(const Chain& chain); + + /** + * Request the total number of joints in the chain.\n + * <strong> Important:</strong> It is not the + * same as the total number of segments since a segment does not + * need to have a joint. This function is important when + * creating a KDL::JntArray to use with this chain. + * @return total nr of joints + */ + unsigned int getNrOfJoints()const {return nrOfJoints;}; + /** + * Request the total number of segments in the chain. + * @return total number of segments + */ + unsigned int getNrOfSegments()const {return nrOfSegments;}; + + /** + * Request the nr'd segment of the chain. There is no boundary + * checking. + * + * @param nr the nr of the segment starting from 0 + * + * @return a constant reference to the nr'd segment + */ + const Segment& getSegment(unsigned int nr)const; + + virtual ~Chain(); + }; + + + +}//end of namespace KDL + +#endif diff --git a/intern/itasc/kdl/chainfksolver.hpp b/intern/itasc/kdl/chainfksolver.hpp new file mode 100644 index 00000000000..fa6f625ee9d --- /dev/null +++ b/intern/itasc/kdl/chainfksolver.hpp @@ -0,0 +1,107 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAIN_FKSOLVER_HPP +#define KDL_CHAIN_FKSOLVER_HPP + +#include "chain.hpp" +#include "framevel.hpp" +#include "frameacc.hpp" +#include "jntarray.hpp" +#include "jntarrayvel.hpp" +#include "jntarrayacc.hpp" + +namespace KDL { + + /** + * \brief This <strong>abstract</strong> class encapsulates a + * solver for the forward position kinematics for a KDL::Chain. + * + * @ingroup KinematicFamily + */ + + //Forward definition + class ChainFkSolverPos { + public: + /** + * Calculate forward position kinematics for a KDL::Chain, + * from joint coordinates to cartesian pose. + * + * @param q_in input joint coordinates + * @param p_out reference to output cartesian pose + * + * @return if < 0 something went wrong + */ + virtual int JntToCart(const JntArray& q_in, Frame& p_out,int segmentNr=-1)=0; + virtual ~ChainFkSolverPos(){}; + }; + + /** + * \brief This <strong>abstract</strong> class encapsulates a solver + * for the forward velocity kinematics for a KDL::Chain. + * + * @ingroup KinematicFamily + */ + class ChainFkSolverVel { + public: + /** + * Calculate forward position and velocity kinematics, from + * joint coordinates to cartesian coordinates. + * + * @param q_in input joint coordinates (position and velocity) + * @param out output cartesian coordinates (position and velocity) + * + * @return if < 0 something went wrong + */ + virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0; + + virtual ~ChainFkSolverVel(){}; + }; + + /** + * \brief This <strong>abstract</strong> class encapsulates a solver + * for the forward acceleration kinematics for a KDL::Chain. + * + * @ingroup KinematicFamily + */ + + class ChainFkSolverAcc { + public: + /** + * Calculate forward position, velocity and accelaration + * kinematics, from joint coordinates to cartesian coordinates + * + * @param q_in input joint coordinates (position, velocity and + * acceleration + @param out output cartesian coordinates (position, velocity + * and acceleration + * + * @return if < 0 something went wrong + */ + virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0; + + virtual ~ChainFkSolverAcc()=0; + }; + + +}//end of namespace KDL + +#endif diff --git a/intern/itasc/kdl/chainfksolverpos_recursive.cpp b/intern/itasc/kdl/chainfksolverpos_recursive.cpp new file mode 100644 index 00000000000..46c29c9c6e0 --- /dev/null +++ b/intern/itasc/kdl/chainfksolverpos_recursive.cpp @@ -0,0 +1,61 @@ +// Copyright (C) 2007 Francois Cauwe <francois at cauwe dot org> +// 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 + +#include "chainfksolverpos_recursive.hpp" +#include <iostream> + +namespace KDL { + + ChainFkSolverPos_recursive::ChainFkSolverPos_recursive(const Chain& _chain): + chain(_chain) + { + } + + int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr) + { + unsigned int segNr = (unsigned int)segmentNr; + if(segmentNr<0) + segNr=chain.getNrOfSegments(); + + p_out = Frame::Identity(); + + if(q_in.rows()!=chain.getNrOfJoints()) + return -1; + else if(segNr>chain.getNrOfSegments()) + return -1; + else{ + int j=0; + for(unsigned int i=0;i<segNr;i++){ + p_out = p_out*chain.getSegment(i).pose(((JntArray&)q_in)(j)); + j+=chain.getSegment(i).getJoint().getNDof(); + } + return 0; + } + } + + + ChainFkSolverPos_recursive::~ChainFkSolverPos_recursive() + { + } + + +} diff --git a/intern/itasc/kdl/chainfksolverpos_recursive.hpp b/intern/itasc/kdl/chainfksolverpos_recursive.hpp new file mode 100644 index 00000000000..72cdab0b28b --- /dev/null +++ b/intern/itasc/kdl/chainfksolverpos_recursive.hpp @@ -0,0 +1,50 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDLCHAINFKSOLVERPOS_RECURSIVE_HPP +#define KDLCHAINFKSOLVERPOS_RECURSIVE_HPP + +#include "chainfksolver.hpp" + +namespace KDL { + + /** + * Implementation of a recursive forward position kinematics + * algorithm to calculate the position transformation from joint + * space to Cartesian space of a general kinematic chain (KDL::Chain). + * + * @ingroup KinematicFamily + */ + class ChainFkSolverPos_recursive : public ChainFkSolverPos + { + public: + ChainFkSolverPos_recursive(const Chain& chain); + ~ChainFkSolverPos_recursive(); + + virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1); + + private: + const Chain chain; + }; + +} + +#endif diff --git a/intern/itasc/kdl/chainjnttojacsolver.cpp b/intern/itasc/kdl/chainjnttojacsolver.cpp new file mode 100644 index 00000000000..4a801c041f3 --- /dev/null +++ b/intern/itasc/kdl/chainjnttojacsolver.cpp @@ -0,0 +1,80 @@ +// 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 + +#include "chainjnttojacsolver.hpp" + +namespace KDL +{ + ChainJntToJacSolver::ChainJntToJacSolver(const Chain& _chain): + chain(_chain) + { + } + + ChainJntToJacSolver::~ChainJntToJacSolver() + { + } + + int ChainJntToJacSolver::JntToJac(const JntArray& q_in,Jacobian& jac) + { + assert(q_in.rows()==chain.getNrOfJoints()&& + q_in.rows()==jac.columns()); + + + Frame T_local, T_joint; + T_total = Frame::Identity(); + SetToZero(t_local); + + int i=chain.getNrOfSegments()-1; + unsigned int q_nr = chain.getNrOfJoints(); + + //Lets recursively iterate until we are in the root segment + while (i >= 0) { + const Segment& segment = chain.getSegment(i); + int ndof = segment.getJoint().getNDof(); + q_nr -= ndof; + + //get the pose of the joint. + T_joint = segment.getJoint().pose(((JntArray&)q_in)(q_nr)); + // combine with the tip to have the tip pose + T_local = T_joint*segment.getFrameToTip(); + //calculate new T_end: + T_total = T_local * T_total; + + for (int dof=0; dof<ndof; dof++) { + // combine joint rotation with tip position to get a reference frame for the joint + T_joint.p = T_local.p; + // in which the twist can be computed (needed for NDof joint) + t_local = segment.twist(T_joint, 1.0, dof); + //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 + t_local = T_total.M.Inverse(t_local); + //store the twist in the jacobian: + jac.twists[q_nr+dof] = t_local; + } + i--; + }//endwhile + //Change the base of the complete jacobian from the endpoint to the base + changeBase(jac, T_total.M, jac); + return 0; + } +} + diff --git a/intern/itasc/kdl/chainjnttojacsolver.hpp b/intern/itasc/kdl/chainjnttojacsolver.hpp new file mode 100644 index 00000000000..98f9d06ee0d --- /dev/null +++ b/intern/itasc/kdl/chainjnttojacsolver.hpp @@ -0,0 +1,65 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAINJNTTOJACSOLVER_HPP +#define KDL_CHAINJNTTOJACSOLVER_HPP + +#include "frames.hpp" +#include "jacobian.hpp" +#include "jntarray.hpp" +#include "chain.hpp" + +namespace KDL +{ + /** + * @brief Class to calculate the jacobian of a general + * KDL::Chain, it is used by other solvers. It should not be used + * outside of KDL. + * + * + */ + + class ChainJntToJacSolver + { + public: + ChainJntToJacSolver(const Chain& chain); + ~ChainJntToJacSolver(); + /** + * Calculate the jacobian expressed in the base frame of the + * chain, with reference point at the end effector of the + * *chain. The alghoritm is similar to the one used in + * KDL::ChainFkSolverVel_recursive + * + * @param q_in input joint positions + * @param jac output jacobian + * + * @return always returns 0 + */ + int JntToJac(const JntArray& q_in,Jacobian& jac); + + private: + const Chain chain; + Twist t_local; + Frame T_total; + }; +} +#endif + diff --git a/intern/itasc/kdl/frameacc.cpp b/intern/itasc/kdl/frameacc.cpp new file mode 100644 index 00000000000..9defce0a00e --- /dev/null +++ b/intern/itasc/kdl/frameacc.cpp @@ -0,0 +1,26 @@ +/***************************************************************************** + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + ****************************************************************************/ + + +#include "frameacc.hpp" + +namespace KDL { + +#ifndef KDL_INLINE + #include "frameacc.inl" +#endif + +} + diff --git a/intern/itasc/kdl/frameacc.hpp b/intern/itasc/kdl/frameacc.hpp new file mode 100644 index 00000000000..4157237222e --- /dev/null +++ b/intern/itasc/kdl/frameacc.hpp @@ -0,0 +1,259 @@ +/***************************************************************************** + * \file + * This file contains the definition of classes for a + * Rall Algebra of (subset of) the classes defined in frames, + * i.e. classes that contain a set (value,derivative,2nd derivative) + * and define operations on that set + * this classes are usefull for automatic differentiation ( <-> symbolic diff , + * <-> numeric diff). + * Defines VectorAcc, RotationAcc, FrameAcc, doubleAcc. + * Look at the corresponding classes Vector Rotation Frame Twist and + * Wrench for the semantics of the methods. + * + * It also contains the 2nd derivative <-> RFrames.h + * + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id: frameacc.hpp 19905 2009-04-23 13:29:54Z ben2610 $ + * $Name: $ + ****************************************************************************/ + +#ifndef RRFRAMES_H +#define RRFRAMES_H + + +#include "utilities/rall2d.h" +#include "frames.hpp" + + + +namespace KDL { + +class TwistAcc; +typedef Rall2d<double,double,double> doubleAcc; + + +class VectorAcc +{ +public: + Vector p; //!< position vector + Vector v; //!< velocity vector + Vector dv; //!< acceleration vector +public: + VectorAcc():p(),v(),dv() {} + explicit VectorAcc(const Vector& _p):p(_p),v(Vector::Zero()),dv(Vector::Zero()) {} + VectorAcc(const Vector& _p,const Vector& _v):p(_p),v(_v),dv(Vector::Zero()) {} + VectorAcc(const Vector& _p,const Vector& _v,const Vector& _dv): + p(_p),v(_v),dv(_dv) {} + IMETHOD VectorAcc& operator = (const VectorAcc& arg); + IMETHOD VectorAcc& operator = (const Vector& arg); + IMETHOD VectorAcc& operator += (const VectorAcc& arg); + IMETHOD VectorAcc& operator -= (const VectorAcc& arg); + IMETHOD static VectorAcc Zero(); + IMETHOD void ReverseSign(); + IMETHOD doubleAcc Norm(); + IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator + (const Vector& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator - (const Vector& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator + (const VectorAcc& r1,const Vector& r2); + IMETHOD friend VectorAcc operator - (const VectorAcc& r1,const Vector& r2); + IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator * (const VectorAcc& r1,const Vector& r2); + IMETHOD friend VectorAcc operator * (const Vector& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator * (const VectorAcc& r1,double r2); + IMETHOD friend VectorAcc operator * (double r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2); + IMETHOD friend VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1); + IMETHOD friend VectorAcc operator*(const Rotation& R,const VectorAcc& x); + + IMETHOD friend VectorAcc operator / (const VectorAcc& r1,double r2); + IMETHOD friend VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1); + + + IMETHOD friend bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon); + IMETHOD friend bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon); + IMETHOD friend bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon); + IMETHOD friend VectorAcc operator - (const VectorAcc& r); + IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs); + IMETHOD friend doubleAcc dot(const VectorAcc& lhs,const Vector& rhs); + IMETHOD friend doubleAcc dot(const Vector& lhs,const VectorAcc& rhs); +}; + + + +class RotationAcc +{ +public: + Rotation R; //!< rotation matrix + Vector w; //!< angular velocity vector + Vector dw; //!< angular acceration vector +public: + RotationAcc():R(),w() {} + explicit RotationAcc(const Rotation& _R):R(_R),w(Vector::Zero()){} + RotationAcc(const Rotation& _R,const Vector& _w,const Vector& _dw): + R(_R),w(_w),dw(_dw) {} + IMETHOD RotationAcc& operator = (const RotationAcc& arg); + IMETHOD RotationAcc& operator = (const Rotation& arg); + IMETHOD static RotationAcc Identity(); + IMETHOD RotationAcc Inverse() const; + IMETHOD VectorAcc Inverse(const VectorAcc& arg) const; + IMETHOD VectorAcc Inverse(const Vector& arg) const; + IMETHOD VectorAcc operator*(const VectorAcc& arg) const; + IMETHOD VectorAcc operator*(const Vector& arg) const; + + // Rotations + // The SetRot.. functions set the value of *this to the appropriate rotation matrix. + // The Rot... static functions give the value of the appropriate rotation matrix back. + // The DoRot... functions apply a rotation R to *this,such that *this = *this * R. + // IMETHOD void DoRotX(const doubleAcc& angle); + // IMETHOD void DoRotY(const doubleAcc& angle); + // IMETHOD void DoRotZ(const doubleAcc& angle); + // IMETHOD static RRotation RotX(const doubleAcc& angle); + // IMETHOD static RRotation RotY(const doubleAcc& angle); + // IMETHOD static RRotation RotZ(const doubleAcc& angle); + + // IMETHOD void SetRot(const Vector& rotaxis,const doubleAcc& angle); + // Along an arbitrary axes. The norm of rotvec is neglected. + // IMETHOD static RotationAcc Rot(const Vector& rotvec,const doubleAcc& angle); + // rotvec has arbitrary norm + // rotation around a constant vector ! + // IMETHOD static RotationAcc Rot2(const Vector& rotvec,const doubleAcc& angle); + // rotvec is normalized. + // rotation around a constant vector ! + + IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2); + IMETHOD friend RotationAcc operator* (const Rotation& r1,const RotationAcc& r2); + IMETHOD friend RotationAcc operator* (const RotationAcc& r1,const Rotation& r2); + IMETHOD friend bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon); + IMETHOD friend bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon); + IMETHOD friend bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon); + IMETHOD TwistAcc Inverse(const TwistAcc& arg) const; + IMETHOD TwistAcc Inverse(const Twist& arg) const; + IMETHOD TwistAcc operator * (const TwistAcc& arg) const; + IMETHOD TwistAcc operator * (const Twist& arg) const; +}; + + + + +class FrameAcc +{ +public: + RotationAcc M; //!< Rotation,angular velocity, and angular acceleration of frame. + VectorAcc p; //!< Translation, velocity and acceleration of origin. +public: + FrameAcc(){} + explicit FrameAcc(const Frame& _T):M(_T.M),p(_T.p) {} + FrameAcc(const Frame& _T,const Twist& _t,const Twist& _dt): + M(_T.M,_t.rot,_dt.rot),p(_T.p,_t.vel,_dt.vel) {} + FrameAcc(const RotationAcc& _M,const VectorAcc& _p):M(_M),p(_p) {} + + IMETHOD FrameAcc& operator = (const FrameAcc& arg); + IMETHOD FrameAcc& operator = (const Frame& arg); + IMETHOD static FrameAcc Identity(); + IMETHOD FrameAcc Inverse() const; + IMETHOD VectorAcc Inverse(const VectorAcc& arg) const; + IMETHOD VectorAcc operator*(const VectorAcc& arg) const; + IMETHOD VectorAcc operator*(const Vector& arg) const; + IMETHOD VectorAcc Inverse(const Vector& arg) const; + IMETHOD Frame GetFrame() const; + IMETHOD Twist GetTwist() const; + IMETHOD Twist GetAccTwist() const; + IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const FrameAcc& f2); + IMETHOD friend FrameAcc operator * (const Frame& f1,const FrameAcc& f2); + IMETHOD friend FrameAcc operator * (const FrameAcc& f1,const Frame& f2); + IMETHOD friend bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon); + IMETHOD friend bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon); + IMETHOD friend bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon); + + IMETHOD TwistAcc Inverse(const TwistAcc& arg) const; + IMETHOD TwistAcc Inverse(const Twist& arg) const; + IMETHOD TwistAcc operator * (const TwistAcc& arg) const; + IMETHOD TwistAcc operator * (const Twist& arg) const; +}; + + + + + + + + +//very similar to Wrench class. +class TwistAcc +{ +public: + VectorAcc vel; //!< translational velocity and its 1st and 2nd derivative + VectorAcc rot; //!< rotational velocity and its 1st and 2nd derivative +public: + + TwistAcc():vel(),rot() {}; + TwistAcc(const VectorAcc& _vel,const VectorAcc& _rot):vel(_vel),rot(_rot) {}; + + IMETHOD TwistAcc& operator-=(const TwistAcc& arg); + IMETHOD TwistAcc& operator+=(const TwistAcc& arg); + + IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,double rhs); + IMETHOD friend TwistAcc operator*(double lhs,const TwistAcc& rhs); + IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,double rhs); + + IMETHOD friend TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs); + IMETHOD friend TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs); + IMETHOD friend TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs); + + IMETHOD friend TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs); + IMETHOD friend TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs); + IMETHOD friend TwistAcc operator-(const TwistAcc& arg); + + IMETHOD friend void SetToZero(TwistAcc& v); + + static IMETHOD TwistAcc Zero(); + + IMETHOD void ReverseSign(); + + IMETHOD TwistAcc RefPoint(const VectorAcc& v_base_AB); + // Changes the reference point of the RTwist. + // The RVector v_base_AB is expressed in the same base as the RTwist + // The RVector v_base_AB is a RVector from the old point to + // the new point. + // Complexity : 6M+6A + + IMETHOD friend bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon); + IMETHOD friend bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon); + IMETHOD friend bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon); + + + IMETHOD Twist GetTwist() const; + IMETHOD Twist GetTwistDot() const; + + friend class RotationAcc; + friend class FrameAcc; + +}; + + + + + + + +#ifdef KDL_INLINE +#include "frameacc.inl" +#endif + +} + + + + + +#endif diff --git a/intern/itasc/kdl/frameacc.inl b/intern/itasc/kdl/frameacc.inl new file mode 100644 index 00000000000..a8ea35ad436 --- /dev/null +++ b/intern/itasc/kdl/frameacc.inl @@ -0,0 +1,598 @@ +/***************************************************************************** + * \file + * provides inline functions of rrframes.h + * + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id: frameacc.inl 19905 2009-04-23 13:29:54Z ben2610 $ + * $Name: $ + ****************************************************************************/ + + + + +/////////////////// VectorAcc ///////////////////////////////////// + +VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2) { + return VectorAcc(r1.p+r2.p,r1.v+r2.v,r1.dv+r2.dv); +} + +VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2) { + return VectorAcc(r1.p-r2.p, r1.v-r2.v, r1.dv-r2.dv); +} +VectorAcc operator + (const Vector& r1,const VectorAcc& r2) { + return VectorAcc(r1+r2.p,r2.v,r2.dv); +} + +VectorAcc operator - (const Vector& r1,const VectorAcc& r2) { + return VectorAcc(r1-r2.p, -r2.v, -r2.dv); +} +VectorAcc operator + (const VectorAcc& r1,const Vector& r2) { + return VectorAcc(r1.p+r2,r1.v,r1.dv); +} + +VectorAcc operator - (const VectorAcc& r1,const Vector& r2) { + return VectorAcc(r1.p-r2, r1.v, r1.dv); +} + +// unary - +VectorAcc operator - (const VectorAcc& r) { + return VectorAcc(-r.p,-r.v,-r.dv); +} + +// cross prod. +VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2) { + return VectorAcc(r1.p*r2.p, + r1.p*r2.v+r1.v*r2.p, + r1.dv*r2.p+2*r1.v*r2.v+r1.p*r2.dv + ); +} + +VectorAcc operator * (const VectorAcc& r1,const Vector& r2) { + return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 ); +} + +VectorAcc operator * (const Vector& r1,const VectorAcc& r2) { + return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv ); +} + + + +// scalar mult. +VectorAcc operator * (double r1,const VectorAcc& r2) { + return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv ); +} + +VectorAcc operator * (const VectorAcc& r1,double r2) { + return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 ); +} + +VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2) { + return VectorAcc(r1.t*r2.p, + r1.t*r2.v + r1.d*r2.p, + r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p + ); +} + +VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1) { + return VectorAcc(r1.t*r2.p, + r1.t*r2.v + r1.d*r2.p, + r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p + ); +} + +VectorAcc& VectorAcc::operator = (const VectorAcc& arg) { + p=arg.p; + v=arg.v; + dv=arg.dv; + return *this; +} + +VectorAcc& VectorAcc::operator = (const Vector& arg) { + p=arg; + v=Vector::Zero(); + dv=Vector::Zero(); + return *this; +} + +VectorAcc& VectorAcc::operator += (const VectorAcc& arg) { + p+=arg.p; + v+=arg.v; + dv+= arg.dv; + return *this; +} +VectorAcc& VectorAcc::operator -= (const VectorAcc& arg) { + p-=arg.p; + v-=arg.v; + dv-=arg.dv; + return *this; +} + +VectorAcc VectorAcc::Zero() { + return VectorAcc(Vector::Zero(),Vector::Zero(),Vector::Zero()); +} + +void VectorAcc::ReverseSign() { + p.ReverseSign(); + v.ReverseSign(); + dv.ReverseSign(); +} + +doubleAcc VectorAcc::Norm() { + doubleAcc res; + res.t = p.Norm(); + res.d = dot(p,v)/res.t; + res.dd = (dot(p,dv)+dot(v,v)-res.d*res.d)/res.t; + return res; +} + +doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs) { + return doubleAcc( dot(lhs.p,rhs.p), + dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p), + dot(lhs.p,rhs.dv)+2*dot(lhs.v,rhs.v)+dot(lhs.dv,rhs.p) + ); +} + +doubleAcc dot(const VectorAcc& lhs,const Vector& rhs) { + return doubleAcc( dot(lhs.p,rhs), + dot(lhs.v,rhs), + dot(lhs.dv,rhs) + ); +} + +doubleAcc dot(const Vector& lhs,const VectorAcc& rhs) { + return doubleAcc( dot(lhs,rhs.p), + dot(lhs,rhs.v), + dot(lhs,rhs.dv) + ); +} + + +bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps) { + return (Equal(r1.p,r2.p,eps) + && Equal(r1.v,r2.v,eps) + && Equal(r1.dv,r2.dv,eps) + ); +} + +bool Equal(const Vector& r1,const VectorAcc& r2,double eps) { + return (Equal(r1,r2.p,eps) + && Equal(Vector::Zero(),r2.v,eps) + && Equal(Vector::Zero(),r2.dv,eps) + ); +} + +bool Equal(const VectorAcc& r1,const Vector& r2,double eps) { + return (Equal(r1.p,r2,eps) + && Equal(r1.v,Vector::Zero(),eps) + && Equal(r1.dv,Vector::Zero(),eps) + ); +} + +VectorAcc operator / (const VectorAcc& r1,double r2) { + return r1*(1.0/r2); +} + +VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1) { + return r2*(1.0/r1); +} + + + +/////////////////// RotationAcc ///////////////////////////////////// + +RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2) { + return RotationAcc( r1.R * r2.R, + r1.w + r1.R*r2.w, + r1.dw + r1.w*(r1.R*r2.w) + r1.R*r2.dw + ); +} + +RotationAcc operator* (const Rotation& r1,const RotationAcc& r2) { + return RotationAcc( r1*r2.R, r1*r2.w, r1*r2.dw); +} + +RotationAcc operator* (const RotationAcc& r1,const Rotation& r2) { + return RotationAcc( r1.R*r2, r1.w, r1.dw ); +} + +RotationAcc& RotationAcc::operator = (const RotationAcc& arg) { + R=arg.R; + w=arg.w; + dw=arg.dw; + return *this; +} +RotationAcc& RotationAcc::operator = (const Rotation& arg) { + R = arg; + w = Vector::Zero(); + dw = Vector::Zero(); + return *this; +} + +RotationAcc RotationAcc::Identity() { + return RotationAcc(Rotation::Identity(),Vector::Zero(),Vector::Zero()); +} + +RotationAcc RotationAcc::Inverse() const { + return RotationAcc(R.Inverse(),-R.Inverse(w),-R.Inverse(dw)); +} + +VectorAcc RotationAcc::Inverse(const VectorAcc& arg) const { + VectorAcc tmp; + tmp.p = R.Inverse(arg.p); + tmp.v = R.Inverse(arg.v - w * arg.p); + tmp.dv = R.Inverse(arg.dv - dw*arg.p - w*(arg.v+R*tmp.v)); + return tmp; +} + +VectorAcc RotationAcc::Inverse(const Vector& arg) const { + VectorAcc tmp; + tmp.p = R.Inverse(arg); + tmp.v = R.Inverse(-w*arg); + tmp.dv = R.Inverse(-dw*arg - w*(R*tmp.v)); + return tmp; +} + + +VectorAcc RotationAcc::operator*(const VectorAcc& arg) const { + VectorAcc tmp; + tmp.p = R*arg.p; + tmp.dv = R*arg.v; + tmp.v = w*tmp.p + tmp.dv; + tmp.dv = dw*tmp.p + w*(tmp.v + tmp.dv) + R*arg.dv; + return tmp; +} + +VectorAcc operator*(const Rotation& R,const VectorAcc& x) { + return VectorAcc(R*x.p,R*x.v,R*x.dv); +} + +VectorAcc RotationAcc::operator*(const Vector& arg) const { + VectorAcc tmp; + tmp.p = R*arg; + tmp.v = w*tmp.p; + tmp.dv = dw*tmp.p + w*tmp.v; + return tmp; +} + +/* + // = Rotations + // The Rot... static functions give the value of the appropriate rotation matrix back. + // The DoRot... functions apply a rotation R to *this,such that *this = *this * R. + + void RRotation::DoRotX(const RDouble& angle) { + w+=R*Vector(angle.grad,0,0); + R.DoRotX(angle.t); + } +RotationAcc RotationAcc::RotX(const doubleAcc& angle) { + return RotationAcc(Rotation::RotX(angle.t), + Vector(angle.d,0,0), + Vector(angle.dd,0,0) + ); +} + + void RRotation::DoRotY(const RDouble& angle) { + w+=R*Vector(0,angle.grad,0); + R.DoRotY(angle.t); + } +RotationAcc RotationAcc::RotY(const doubleAcc& angle) { + return RotationAcc( + Rotation::RotX(angle.t), + Vector(0,angle.d,0), + Vector(0,angle.dd,0) + ); +} + + void RRotation::DoRotZ(const RDouble& angle) { + w+=R*Vector(0,0,angle.grad); + R.DoRotZ(angle.t); + } +RotationAcc RotationAcc::RotZ(const doubleAcc& angle) { + return RotationAcc( + Rotation::RotZ(angle.t), + Vector(0,0,angle.d), + Vector(0,0,angle.dd) + ); +} + + + RRotation RRotation::Rot(const Vector& rotvec,const RDouble& angle) + // rotvec has arbitrary norm + // rotation around a constant vector ! + { + Vector v = rotvec.Normalize(); + return RRotation(Rotation::Rot2(v,angle.t),v*angle.grad); + } + + RRotation RRotation::Rot2(const Vector& rotvec,const RDouble& angle) + // rotvec is normalized. + { + return RRotation(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad); + } + +*/ + +bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps) { + return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps) && Equal(r1.dw,r2.dw,eps) ); +} +bool Equal(const Rotation& r1,const RotationAcc& r2,double eps) { + return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps) && + Equal(Vector::Zero(),r2.dw,eps) ); +} +bool Equal(const RotationAcc& r1,const Rotation& r2,double eps) { + return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps) && + Equal(r1.dw,Vector::Zero(),eps) ); +} + + +// Methods and operators related to FrameAcc +// They all delegate most of the work to RotationAcc and VectorAcc +FrameAcc& FrameAcc::operator = (const FrameAcc& arg) { + M=arg.M; + p=arg.p; + return *this; +} + +FrameAcc FrameAcc::Identity() { + return FrameAcc(RotationAcc::Identity(),VectorAcc::Zero()); +} + + +FrameAcc operator *(const FrameAcc& lhs,const FrameAcc& rhs) +{ + return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} +FrameAcc operator *(const FrameAcc& lhs,const Frame& rhs) +{ + return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} +FrameAcc operator *(const Frame& lhs,const FrameAcc& rhs) +{ + return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} + +VectorAcc FrameAcc::operator *(const VectorAcc & arg) const +{ + return M*arg+p; +} +VectorAcc FrameAcc::operator *(const Vector & arg) const +{ + return M*arg+p; +} + +VectorAcc FrameAcc::Inverse(const VectorAcc& arg) const +{ + return M.Inverse(arg-p); +} + +VectorAcc FrameAcc::Inverse(const Vector& arg) const +{ + return M.Inverse(arg-p); +} + +FrameAcc FrameAcc::Inverse() const +{ + return FrameAcc(M.Inverse(),-M.Inverse(p)); +} + +FrameAcc& FrameAcc::operator =(const Frame & arg) +{ + M = arg.M; + p = arg.p; + return *this; +} + +bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} +bool Equal(const Frame& r1,const FrameAcc& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} +bool Equal(const FrameAcc& r1,const Frame& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} + + +Frame FrameAcc::GetFrame() const { + return Frame(M.R,p.p); +} + + +Twist FrameAcc::GetTwist() const { + return Twist(p.v,M.w); +} + + +Twist FrameAcc::GetAccTwist() const { + return Twist(p.dv,M.dw); +} + + + + + + + + + + + + + + + + + +TwistAcc TwistAcc::Zero() +{ + return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero()); +} + + +void TwistAcc::ReverseSign() +{ + vel.ReverseSign(); + rot.ReverseSign(); +} + +TwistAcc TwistAcc::RefPoint(const VectorAcc& v_base_AB) + // Changes the reference point of the TwistAcc. + // The RVector v_base_AB is expressed in the same base as the TwistAcc + // The RVector v_base_AB is a RVector from the old point to + // the new point. + // Complexity : 6M+6A +{ + return TwistAcc(this->vel+this->rot*v_base_AB,this->rot); +} + +TwistAcc& TwistAcc::operator-=(const TwistAcc& arg) +{ + vel-=arg.vel; + rot -=arg.rot; + return *this; +} + +TwistAcc& TwistAcc::operator+=(const TwistAcc& arg) +{ + vel+=arg.vel; + rot +=arg.rot; + return *this; +} + + +TwistAcc operator*(const TwistAcc& lhs,double rhs) +{ + return TwistAcc(lhs.vel*rhs,lhs.rot*rhs); +} + +TwistAcc operator*(double lhs,const TwistAcc& rhs) +{ + return TwistAcc(lhs*rhs.vel,lhs*rhs.rot); +} + +TwistAcc operator/(const TwistAcc& lhs,double rhs) +{ + return TwistAcc(lhs.vel/rhs,lhs.rot/rhs); +} + + +TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs) +{ + return TwistAcc(lhs.vel*rhs,lhs.rot*rhs); +} + +TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs) +{ + return TwistAcc(lhs*rhs.vel,lhs*rhs.rot); +} + +TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs) +{ + return TwistAcc(lhs.vel/rhs,lhs.rot/rhs); +} + + + +// addition of TwistAcc's +TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs) +{ + return TwistAcc(lhs.vel+rhs.vel,lhs.rot+rhs.rot); +} + +TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs) +{ + return TwistAcc(lhs.vel-rhs.vel,lhs.rot-rhs.rot); +} + +// unary - +TwistAcc operator-(const TwistAcc& arg) +{ + return TwistAcc(-arg.vel,-arg.rot); +} + + + + + +TwistAcc RotationAcc::Inverse(const TwistAcc& arg) const +{ + return TwistAcc(Inverse(arg.vel),Inverse(arg.rot)); +} + +TwistAcc RotationAcc::operator * (const TwistAcc& arg) const +{ + return TwistAcc((*this)*arg.vel,(*this)*arg.rot); +} + +TwistAcc RotationAcc::Inverse(const Twist& arg) const +{ + return TwistAcc(Inverse(arg.vel),Inverse(arg.rot)); +} + +TwistAcc RotationAcc::operator * (const Twist& arg) const +{ + return TwistAcc((*this)*arg.vel,(*this)*arg.rot); +} + + +TwistAcc FrameAcc::operator * (const TwistAcc& arg) const +{ + TwistAcc tmp; + tmp.rot = M*arg.rot; + tmp.vel = M*arg.vel+p*tmp.rot; + return tmp; +} + +TwistAcc FrameAcc::operator * (const Twist& arg) const +{ + TwistAcc tmp; + tmp.rot = M*arg.rot; + tmp.vel = M*arg.vel+p*tmp.rot; + return tmp; +} + +TwistAcc FrameAcc::Inverse(const TwistAcc& arg) const +{ + TwistAcc tmp; + tmp.rot = M.Inverse(arg.rot); + tmp.vel = M.Inverse(arg.vel-p*arg.rot); + return tmp; +} + +TwistAcc FrameAcc::Inverse(const Twist& arg) const +{ + TwistAcc tmp; + tmp.rot = M.Inverse(arg.rot); + tmp.vel = M.Inverse(arg.vel-p*arg.rot); + return tmp; +} + +Twist TwistAcc::GetTwist() const { + return Twist(vel.p,rot.p); +} + +Twist TwistAcc::GetTwistDot() const { + return Twist(vel.v,rot.v); +} + +bool Equal(const TwistAcc& a,const TwistAcc& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} +bool Equal(const Twist& a,const TwistAcc& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} +bool Equal(const TwistAcc& a,const Twist& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} + diff --git a/intern/itasc/kdl/frames.cpp b/intern/itasc/kdl/frames.cpp new file mode 100644 index 00000000000..7dcc39f2cd4 --- /dev/null +++ b/intern/itasc/kdl/frames.cpp @@ -0,0 +1,389 @@ +/*************************************************************************** + frames.cxx - description + ------------------------- + begin : June 2006 + copyright : (C) 2006 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 "frames.hpp" + +namespace KDL { + +#ifndef KDL_INLINE +#include "frames.inl" +#endif + +void Frame::Make4x4(double * d) +{ + int i; + int j; + 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; + } + for (j=0;j<3;j++) + d[12+j] = 0.; + d[15] = 1; +} + +Frame Frame::DH_Craig1989(double a,double alpha,double d,double theta) +// returns Modified Denavit-Hartenberg parameters (According to Craig) +{ + double ct,st,ca,sa; + ct = cos(theta); + st = sin(theta); + sa = sin(alpha); + ca = cos(alpha); + return Frame(Rotation( + ct, -st, 0, + st*ca, ct*ca, -sa, + st*sa, ct*sa, ca ), + Vector( + a, -sa*d, ca*d ) + ); +} + +Frame Frame::DH(double a,double alpha,double d,double theta) +// returns Denavit-Hartenberg parameters (Non-Modified DH) +{ + double ct,st,ca,sa; + ct = cos(theta); + st = sin(theta); + sa = sin(alpha); + ca = cos(alpha); + return Frame(Rotation( + ct, -st*ca, st*sa, + st, ct*ca, -ct*sa, + 0, sa, ca ), + Vector( + a*ct, a*st, d ) + ); +} + +double Vector2::Norm() const +{ + double tmp0 = fabs(data[0]); + double tmp1 = fabs(data[1]); + if (tmp0 >= tmp1) { + if (tmp1 == 0) + return 0; + return tmp0*sqrt(1+sqr(tmp1/tmp0)); + } else { + return tmp1*sqrt(1+sqr(tmp0/tmp1)); + } +} +// makes v a unitvector and returns the norm of v. +// if v is smaller than eps, Vector(1,0,0) is returned with norm 0. +// if this is not good, check the return value of this method. +double Vector2::Normalize(double eps) { + double v = this->Norm(); + if (v < eps) { + *this = Vector2(1,0); + return v; + } else { + *this = (*this)/v; + return v; + } +} + + +// do some effort not to lose precision +double Vector::Norm() const +{ + double tmp1; + double tmp2; + tmp1 = fabs(data[0]); + tmp2 = fabs(data[1]); + if (tmp1 >= tmp2) { + tmp2=fabs(data[2]); + if (tmp1 >= tmp2) { + if (tmp1 == 0) { + // only to everything exactly zero case, all other are handled correctly + return 0; + } + return tmp1*sqrt(1+sqr(data[1]/data[0])+sqr(data[2]/data[0])); + } else { + return tmp2*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2])); + } + } else { + tmp1=fabs(data[2]); + if (tmp2 > tmp1) { + return tmp2*sqrt(1+sqr(data[0]/data[1])+sqr(data[2]/data[1])); + } else { + return tmp1*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2])); + } + } +} + +// makes v a unitvector and returns the norm of v. +// if v is smaller than eps, Vector(1,0,0) is returned with norm 0. +// if this is not good, check the return value of this method. +double Vector::Normalize(double eps) { + double v = this->Norm(); + if (v < eps) { + *this = Vector(1,0,0); + return v; + } else { + *this = (*this)/v; + return v; + } +} + + +bool Equal(const Rotation& a,const Rotation& b,double eps) { + return (Equal(a.data[0],b.data[0],eps) && + Equal(a.data[1],b.data[1],eps) && + Equal(a.data[2],b.data[2],eps) && + Equal(a.data[3],b.data[3],eps) && + Equal(a.data[4],b.data[4],eps) && + Equal(a.data[5],b.data[5],eps) && + Equal(a.data[6],b.data[6],eps) && + Equal(a.data[7],b.data[7],eps) && + Equal(a.data[8],b.data[8],eps) ); +} + +void Rotation::Ortho() +{ + double n; + n=sqrt(sqr(data[0])+sqr(data[3])+sqr(data[6]));n=(n>1e-10)?1.0/n:0.0;data[0]*=n;data[3]*=n;data[6]*=n; + n=sqrt(sqr(data[1])+sqr(data[4])+sqr(data[7]));n=(n>1e-10)?1.0/n:0.0;data[1]*=n;data[4]*=n;data[7]*=n; + n=sqrt(sqr(data[2])+sqr(data[5])+sqr(data[8]));n=(n>1e-10)?1.0/n:0.0;data[2]*=n;data[5]*=n;data[8]*=n; +} + +Rotation operator *(const Rotation& lhs,const Rotation& rhs) +// Complexity : 27M+27A +{ + return Rotation( + lhs.data[0]*rhs.data[0]+lhs.data[1]*rhs.data[3]+lhs.data[2]*rhs.data[6], + lhs.data[0]*rhs.data[1]+lhs.data[1]*rhs.data[4]+lhs.data[2]*rhs.data[7], + lhs.data[0]*rhs.data[2]+lhs.data[1]*rhs.data[5]+lhs.data[2]*rhs.data[8], + lhs.data[3]*rhs.data[0]+lhs.data[4]*rhs.data[3]+lhs.data[5]*rhs.data[6], + lhs.data[3]*rhs.data[1]+lhs.data[4]*rhs.data[4]+lhs.data[5]*rhs.data[7], + lhs.data[3]*rhs.data[2]+lhs.data[4]*rhs.data[5]+lhs.data[5]*rhs.data[8], + lhs.data[6]*rhs.data[0]+lhs.data[7]*rhs.data[3]+lhs.data[8]*rhs.data[6], + lhs.data[6]*rhs.data[1]+lhs.data[7]*rhs.data[4]+lhs.data[8]*rhs.data[7], + lhs.data[6]*rhs.data[2]+lhs.data[7]*rhs.data[5]+lhs.data[8]*rhs.data[8] + ); + +} + + +Rotation Rotation::RPY(double roll,double pitch,double yaw) + { + double ca1,cb1,cc1,sa1,sb1,sc1; + ca1 = cos(yaw); sa1 = sin(yaw); + cb1 = cos(pitch);sb1 = sin(pitch); + cc1 = cos(roll);sc1 = sin(roll); + return Rotation(ca1*cb1,ca1*sb1*sc1 - sa1*cc1,ca1*sb1*cc1 + sa1*sc1, + sa1*cb1,sa1*sb1*sc1 + ca1*cc1,sa1*sb1*cc1 - ca1*sc1, + -sb1,cb1*sc1,cb1*cc1); + } + +// 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 ; + } else { + roll = atan2(data[7], data[8]); + pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) ); + yaw = atan2(data[3], data[0]); + } + } + +Rotation Rotation::EulerZYZ(double Alfa,double Beta,double Gamma) { + double sa,ca,sb,cb,sg,cg; + sa = sin(Alfa);ca = cos(Alfa); + sb = sin(Beta);cb = cos(Beta); + sg = sin(Gamma);cg = cos(Gamma); + return Rotation( ca*cb*cg-sa*sg, -ca*cb*sg-sa*cg, ca*sb, + sa*cb*cg+ca*sg, -sa*cb*sg+ca*cg, sa*sb, + -sb*cg , sb*sg, cb + ); + + } + + +void Rotation::GetEulerZYZ(double& alfa,double& beta,double& gamma) const { + if (fabs(data[6]) < epsilon ) { + alfa=0.0; + if (data[8]>0) { + beta = 0.0; + gamma= atan2(-data[1],data[0]); + } else { + beta = PI; + gamma= atan2(data[1],-data[0]); + } + } else { + alfa=atan2(data[5], data[2]); + beta=atan2(sqrt( sqr(data[6]) +sqr(data[7]) ),data[8]); + gamma=atan2(data[7], -data[6]); + } + } + +Rotation Rotation::Rot(const Vector& rotaxis,double angle) { + // The formula is + // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr)) + // can be found by multiplying it with an arbitrary vector p + // and noting that this vector is rotated. + double ct = cos(angle); + double st = sin(angle); + double vt = 1-ct; + Vector rotvec = rotaxis; + rotvec.Normalize(); + return Rotation( + ct + vt*rotvec(0)*rotvec(0), + -rotvec(2)*st + vt*rotvec(0)*rotvec(1), + rotvec(1)*st + vt*rotvec(0)*rotvec(2), + rotvec(2)*st + vt*rotvec(1)*rotvec(0), + ct + vt*rotvec(1)*rotvec(1), + -rotvec(0)*st + vt*rotvec(1)*rotvec(2), + -rotvec(1)*st + vt*rotvec(2)*rotvec(0), + rotvec(0)*st + vt*rotvec(2)*rotvec(1), + ct + vt*rotvec(2)*rotvec(2) + ); + } + +Rotation Rotation::Rot2(const Vector& rotvec,double angle) { + // rotvec should be normalized ! + // The formula is + // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr)) + // can be found by multiplying it with an arbitrary vector p + // and noting that this vector is rotated. + double ct = cos(angle); + double st = sin(angle); + double vt = 1-ct; + return Rotation( + ct + vt*rotvec(0)*rotvec(0), + -rotvec(2)*st + vt*rotvec(0)*rotvec(1), + rotvec(1)*st + vt*rotvec(0)*rotvec(2), + rotvec(2)*st + vt*rotvec(1)*rotvec(0), + ct + vt*rotvec(1)*rotvec(1), + -rotvec(0)*st + vt*rotvec(1)*rotvec(2), + -rotvec(1)*st + vt*rotvec(2)*rotvec(0), + rotvec(0)*st + vt*rotvec(2)*rotvec(1), + ct + vt*rotvec(2)*rotvec(2) + ); +} + + + +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 { + if (ca < 0.0) { + alfa = KDL::PI; + axis.data[0] = 0.0; + axis.data[1] = 0.0; + axis.data[2] = 0.0; + if (data[0] > 0.0) { + axis.data[0] = 1.0; + } else if (data[4] > 0.0) { + axis.data[1] = 1.0; + } else { + axis.data[2] = 1.0; + } + } else { + alfa = 0.0; + } + } + return axis * alfa; + } + +Vector2 Rotation::GetXZRot() const +{ + // [0,1,0] x Y + Vector2 axis(data[7], -data[1]); + double norm = axis.Normalize(); + if (norm < epsilon) { + norm = (data[4] < 0.0) ? PI : 0.0; + } else { + norm = acos(data[4]); + } + return axis*norm; +} + + +/** 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 + * in the case of angle == PI : 2 solutions, positive Z-component + * of the axis is choosen. + * @result returns the rotation angle (between [0..PI] ) + * /todo : + * Check corresponding routines in rframes and rrframes + */ +double Rotation::GetRotAngle(Vector& axis,double eps) const { + double ca = (data[0]+data[4]+data[8]-1)/2.0; + if (ca>1-eps) { + // undefined choose the Z-axis, and angle 0 + axis = Vector(0,0,1); + return 0; + } + if (ca < -1+eps) { + // 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; + 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; +} + +bool operator==(const Rotation& a,const Rotation& b) { +#ifdef KDL_USE_EQUAL + return Equal(a,b); +#else + return ( a.data[0]==b.data[0] && + a.data[1]==b.data[1] && + a.data[2]==b.data[2] && + a.data[3]==b.data[3] && + a.data[4]==b.data[4] && + a.data[5]==b.data[5] && + a.data[6]==b.data[6] && + a.data[7]==b.data[7] && + a.data[8]==b.data[8] ); +#endif +} +} diff --git a/intern/itasc/kdl/frames.hpp b/intern/itasc/kdl/frames.hpp new file mode 100644 index 00000000000..20590c5303e --- /dev/null +++ b/intern/itasc/kdl/frames.hpp @@ -0,0 +1,1097 @@ +/*************************************************************************** + frames.hpp `- description + ------------------------- + begin : June 2006 + copyright : (C) 2006 Erwin Aertbelien + email : firstname.lastname@mech.kuleuven.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 * + * * + ***************************************************************************/ + +/** + * \file + * \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 : + * Any consistent series of units can be used, e.g. N,mm,Nmm,..mm/sec + * + * \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 + * + * \par Complexity + * Sometimes the amount of work is given in the documentation + * e.g. 6M+3A means 6 multiplications and 3 additions. + * + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + ****************************************************************************/ +#ifndef KDL_FRAMES_H +#define KDL_FRAMES_H + + +#include "utilities/kdl-config.h" +#include "utilities/utility.h" + +///////////////////////////////////////////////////////////// + +namespace KDL { + + + +class Vector; +class Rotation; +class Frame; +class Wrench; +class Twist; +class Vector2; +class Rotation2; +class Frame2; + + + +/** + * \brief A concrete implementation of a 3 dimensional vector class + */ +class Vector +{ +public: + double data[3]; + //! Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that + inline Vector() {data[0]=data[1]=data[2] = 0.0;} + + //! Constructs a vector out of the three values x, y and z + inline Vector(double x,double y, double z); + + //! Constructs a vector out of an array of three values x, y and z + inline Vector(double* xyz); + + //! Constructs a vector out of an array of three values x, y and z + inline Vector(float* xyz); + + //! Assignment operator. The normal copy by value semantics. + inline Vector(const Vector& arg); + + //! store vector components in array + inline void GetValue(double* xyz) const; + + //! Assignment operator. The normal copy by value semantics. + inline Vector& operator = ( const Vector& arg); + + //! Access to elements, range checked when NDEBUG is not set, from 0..2 + inline double operator()(int index) const; + + //! Access to elements, range checked when NDEBUG is not set, from 0..2 + inline double& operator() (int index); + + //! Equivalent to double operator()(int index) const + double operator[] ( int index ) const + { + return this->operator() ( index ); + } + + //! Equivalent to double& operator()(int index) + double& operator[] ( int index ) + { + return this->operator() ( index ); + } + + inline double x() const; + inline double y() const; + inline double z() const; + inline void x(double); + inline void y(double); + inline void z(double); + + //! Reverses the sign of the Vector object itself + inline void ReverseSign(); + + + //! subtracts a vector from the Vector object itself + inline Vector& operator-=(const Vector& arg); + + + //! Adds a vector from the Vector object itself + inline Vector& operator +=(const Vector& arg); + + //! Scalar multiplication is defined + inline friend Vector operator*(const Vector& lhs,double rhs); + //! Scalar multiplication is defined + inline friend Vector operator*(double lhs,const Vector& rhs); + //! Scalar division is defined + + inline friend Vector operator/(const Vector& lhs,double rhs); + inline friend Vector operator+(const Vector& lhs,const Vector& rhs); + inline friend Vector operator-(const Vector& lhs,const Vector& rhs); + inline friend Vector operator*(const Vector& lhs,const Vector& rhs); + inline friend Vector operator-(const Vector& arg); + inline friend double dot(const Vector& lhs,const Vector& rhs); + + //! To have a uniform operator to put an element to zero, for scalar values + //! and for objects. + inline friend void SetToZero(Vector& v); + + //! @return a zero vector + inline static Vector Zero(); + + /** Normalizes this vector and returns it norm + * makes v a unitvector and returns the norm of v. + * if v is smaller than eps, Vector(1,0,0) is returned with norm 0. + * if this is not good, check the return value of this method. + */ + double Normalize(double eps=epsilon); + + //! @return the norm of the vector + double Norm() const; + + + + //! a 3D vector where the 2D vector v is put in the XY plane + inline void Set2DXY(const Vector2& v); + //! a 3D vector where the 2D vector v is put in the YZ plane + inline void Set2DYZ(const Vector2& v); + //! a 3D vector where the 2D vector v is put in the ZX plane + inline void Set2DZX(const Vector2& v); + //! a 3D vector where the 2D vector v_XY is put in the XY plane of the frame F_someframe_XY. + inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY); + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Vector& a,const Vector& b,double eps=epsilon); + + //! return a normalized vector + inline friend Vector Normalize(const Vector& a, double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const Vector& a,const Vector& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const Vector& a,const Vector& b); + + friend class Rotation; + friend class Frame; +}; + + +/** + \brief represents rotations in 3 dimensional space. + + This class represents a rotation matrix with the following + conventions : + \verbatim + Suppose V2 = R*V, (1) + V is expressed in frame B + V2 is expressed in frame A + This matrix R consists of 3 collumns [ X,Y,Z ], + X,Y, and Z contain the axes of frame B, expressed in frame A + Because of linearity expr(1) is valid. + \endverbatim + This class only represents rotational_interpolation, not translation + Two interpretations are possible for rotation angles. + * if you rotate with angle around X frame A to have frame B, + then the result of SetRotX is equal to frame B expressed wrt A. + In code: + \verbatim + Rotation R; + F_A_B = R.SetRotX(angle); + \endverbatim + * Secondly, if you take the following code : + \verbatim + Vector p,p2; Rotation R; + R.SetRotX(angle); + p2 = R*p; + \endverbatim + then the frame p2 is rotated around X axis with (-angle). + Analogue reasonings can be applyd to SetRotY,SetRotZ,SetRot + \par type + Concrete implementation +*/ +class Rotation +{ +public: + double data[9]; + + inline Rotation() { + *this = Rotation::Identity(); + } + inline Rotation(double Xx,double Yx,double Zx, + double Xy,double Yy,double Zy, + double Xz,double Yz,double Zz); + inline Rotation(const Vector& x,const Vector& y,const Vector& z); + // default copy constructor is sufficient + + inline void setValue(float* oglmat); + inline void getValue(float* oglmat) const; + + inline Rotation& operator=(const Rotation& arg); + + //! Defines a multiplication R*V between a Rotation R and a Vector V. + //! Complexity : 9M+6A + inline Vector operator*(const Vector& v) const; + + //! Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set + inline double& operator()(int i,int j); + + //! Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set + inline double operator() (int i,int j) const; + + friend Rotation operator *(const Rotation& lhs,const Rotation& rhs); + + //! Sets the value of *this to its inverse. + inline void SetInverse(); + + //! Gives back the inverse rotation matrix of *this. + inline Rotation Inverse() const; + + //! The same as R.Inverse()*v but more efficient. + inline Vector Inverse(const Vector& v) const; + + //! The same as R.Inverse()*arg but more efficient. + inline Wrench Inverse(const Wrench& arg) const; + + //! The same as R.Inverse()*arg but more efficient. + inline Twist Inverse(const Twist& arg) const; + + //! Gives back an identity rotaton matrix + inline static Rotation Identity(); + + +// = Rotations + //! The Rot... static functions give the value of the appropriate rotation matrix back. + inline static Rotation RotX(double angle); + //! The Rot... static functions give the value of the appropriate rotation matrix back. + inline static Rotation RotY(double angle); + //! The Rot... static functions give the value of the appropriate rotation matrix back. + inline static Rotation RotZ(double angle); + //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.. + //! DoRot... functions are only defined when they can be executed more efficiently + inline void DoRotX(double angle); + //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.. + //! DoRot... functions are only defined when they can be executed more efficiently + inline void DoRotY(double angle); + //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.. + //! DoRot... functions are only defined when they can be executed more efficiently + inline void DoRotZ(double angle); + + //! Along an arbitrary axes. It is not necessary to normalize rotaxis. + //! returns identity rotation matrix in the case that the norm of rotaxis + //! is to small to be used. + // @see Rot2 if you want to handle this error in another way. + static Rotation Rot(const Vector& rotaxis,double angle); + + //! Along an arbitrary axes. rotvec should be normalized. + static Rotation Rot2(const Vector& rotvec,double angle); + + // make sure the matrix is a pure rotation (no scaling) + void Ortho(); + + //! Returns a vector with the direction of the equiv. axis + //! and its norm is angle + Vector GetRot() const; + + //! Returns a 2D vector representing the equivalent rotation in the XZ plane that brings the + //! Y axis onto the Matrix Y axis and its norm is angle + Vector2 GetXZRot() 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 + * in the case of angle == PI : 2 solutions, positive Z-component + * of the axis is choosen. + * @result returns the rotation angle (between [0..PI] ) + */ + 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. + 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; + + + //! 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 + 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 + 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 + 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. + inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const { + GetRPY(Gamma,Beta,Alfa); + } + + //! Transformation of the base to which the twist is expressed. + //! Complexity : 18M+12A + //! @see Frame*Twist for a transformation that also transforms + //! the velocity reference point. + inline Twist operator * (const Twist& arg) const; + + //! Transformation of the base to which the wrench is expressed. + //! Complexity : 18M+12A + //! @see Frame*Wrench for a transformation that also transforms + //! the force reference point. + inline Wrench operator * (const Wrench& arg) const; + + //! Access to the underlying unitvectors of the rotation matrix + inline Vector UnitX() const { + return Vector(data[0],data[3],data[6]); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline void UnitX(const Vector& X) { + data[0] = X(0); + data[3] = X(1); + data[6] = X(2); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline Vector UnitY() const { + return Vector(data[1],data[4],data[7]); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline void UnitY(const Vector& X) { + data[1] = X(0); + data[4] = X(1); + data[7] = X(2); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline Vector UnitZ() const { + return Vector(data[2],data[5],data[8]); + } + + //! Access to the underlying unitvectors of the rotation matrix + inline void UnitZ(const Vector& X) { + data[2] = X(0); + data[5] = X(1); + data[8] = X(2); + } + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + friend bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + friend bool operator==(const Rotation& a,const Rotation& b); + //! The literal inequality operator!=() + friend bool operator!=(const Rotation& a,const Rotation& b); + + friend class Frame; +}; + bool operator==(const Rotation& a,const Rotation& b); + + + +/** + \brief represents a frame transformation in 3D space (rotation + translation) + + if V2 = Frame*V1 (V2 expressed in frame A, V1 expressed in frame B) + then V2 = Frame.M*V1+Frame.p + + Frame.M contains columns that represent the axes of frame B wrt frame A + Frame.p contains the origin of frame B expressed in frame A. +*/ +class Frame { +public: + Vector p; //!< origine of the Frame + Rotation M; //!< Orientation of the Frame + +public: + + inline Frame(const Rotation& R,const Vector& V); + + //! The rotation matrix defaults to identity + explicit inline Frame(const Vector& V); + //! The position matrix defaults to zero + explicit inline Frame(const Rotation& R); + + inline void setValue(float* oglmat); + inline void getValue(float* oglmat) const; + + inline Frame() {} + //! The copy constructor. Normal copy by value semantics. + inline Frame(const Frame& arg); + + //! Reads data from an double array + //\TODO should be formulated as a constructor + void Make4x4(double* d); + + //! Treats a frame as a 4x4 matrix and returns element i,j + //! Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set + inline double operator()(int i,int j); + + //! Treats a frame as a 4x4 matrix and returns element i,j + //! Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set + inline double operator() (int i,int j) const; + + // = Inverse + //! Gives back inverse transformation of a Frame + inline Frame Inverse() const; + + //! The same as p2=R.Inverse()*p but more efficient. + inline Vector Inverse(const Vector& arg) const; + + //! The same as p2=R.Inverse()*p but more efficient. + inline Wrench Inverse(const Wrench& arg) const; + + //! The same as p2=R.Inverse()*p but more efficient. + inline Twist Inverse(const Twist& arg) const; + + //! Normal copy-by-value semantics. + inline Frame& operator = (const Frame& arg); + + //! Transformation of the base to which the vector + //! is expressed. + inline Vector operator * (const Vector& arg) const; + + //! Transformation of both the force reference point + //! and of the base to which the wrench is expressed. + //! look at Rotation*Wrench operator for a transformation + //! of only the base to which the twist is expressed. + //! + //! Complexity : 24M+18A + inline Wrench operator * (const Wrench& arg) const; + + //! Transformation of both the velocity reference point + //! and of the base to which the twist is expressed. + //! look at Rotation*Twist for a transformation of only the + //! base to which the twist is expressed. + //! + //! Complexity : 24M+18A + inline Twist operator * (const Twist& arg) const; + + //! Composition of two frames. + inline friend Frame operator *(const Frame& lhs,const Frame& rhs); + + //! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()). + inline static Frame Identity(); + + //! 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. + inline void Integrate(const Twist& t_this,double frequency); + + /* + // DH_Craig1989 : constructs a transformationmatrix + // T_link(i-1)_link(i) with the Denavit-Hartenberg convention as + // described in the Craigs book: Craig, J. J.,Introduction to + // Robotics: Mechanics and Control, Addison-Wesley, + // isbn:0-201-10326-5, 1986. + // + // Note that the frame is a redundant way to express the information + // in the DH-convention. + // \verbatim + // Parameters in full : a(i-1),alpha(i-1),d(i),theta(i) + // + // axis i-1 is connected by link i-1 to axis i numbering axis 1 + // to axis n link 0 (immobile base) to link n + // + // link length a(i-1) length of the mutual perpendicular line + // (normal) between the 2 axes. This normal runs from (i-1) to + // (i) axis. + // + // link twist alpha(i-1): construct plane perpendicular to the + // normal project axis(i-1) and axis(i) into plane angle from + // (i-1) to (i) measured in the direction of the normal + // + // link offset d(i) signed distance between normal (i-1) to (i) + // and normal (i) to (i+1) along axis i joint angle theta(i) + // signed angle between normal (i-1) to (i) and normal (i) to + // (i+1) along axis i + // + // First and last joints : a(0)= a(n) = 0 + // alpha(0) = alpha(n) = 0 + // + // PRISMATIC : theta(1) = 0 d(1) arbitrarily + // + // REVOLUTE : theta(1) arbitrarily d(1) = 0 + // + // Not unique : if intersecting joint axis 2 choices for normal + // Frame assignment of the DH convention : Z(i-1) follows axis + // (i-1) X(i-1) is the normal between axis(i-1) and axis(i) + // Y(i-1) follows out of Z(i-1) and X(i-1) + // + // a(i-1) = distance from Z(i-1) to Z(i) along X(i-1) + // alpha(i-1) = angle between Z(i-1) to Z(i) along X(i-1) + // d(i) = distance from X(i-1) to X(i) along Z(i) + // theta(i) = angle between X(i-1) to X(i) along X(i) + // \endverbatim + */ + static Frame DH_Craig1989(double a,double alpha,double d,double theta); + + // DH : constructs a transformationmatrix T_link(i-1)_link(i) with + // the Denavit-Hartenberg convention as described in the original + // publictation: Denavit, J. and Hartenberg, R. S., A kinematic + // notation for lower-pair mechanisms based on matrices, ASME + // Journal of Applied Mechanics, 23:215-221, 1955. + + static Frame DH(double a,double alpha,double d,double theta); + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Frame& a,const Frame& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const Frame& a,const Frame& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const Frame& a,const Frame& b); +}; + +/** + * \brief represents both translational and rotational velocities. + * + * This class represents a twist. A twist is the combination of translational + * velocity and rotational velocity applied at one point. +*/ +class Twist { +public: + Vector vel; //!< The velocity of that point + Vector rot; //!< The rotational velocity of that point. +public: + + //! The default constructor initialises to Zero via the constructor of Vector. + Twist():vel(),rot() {}; + + Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {}; + + inline Twist& operator-=(const Twist& arg); + inline Twist& operator+=(const Twist& arg); + //! index-based access to components, first vel(0..2), then rot(3..5) + inline double& operator()(int i); + + //! index-based access to components, first vel(0..2), then rot(3..5) + //! For use with a const Twist + inline double operator()(int i) const; + + double operator[] ( int index ) const + { + return this->operator() ( index ); + } + + double& operator[] ( int index ) + { + return this->operator() ( index ); + } + + inline friend Twist operator*(const Twist& lhs,double rhs); + inline friend Twist operator*(double lhs,const Twist& rhs); + inline friend Twist operator/(const Twist& lhs,double rhs); + inline friend Twist operator+(const Twist& lhs,const Twist& rhs); + inline friend Twist operator-(const Twist& lhs,const Twist& rhs); + inline friend Twist operator-(const Twist& arg); + inline friend double dot(const Twist& lhs,const Wrench& rhs); + inline friend double dot(const Wrench& rhs,const Twist& lhs); + inline friend void SetToZero(Twist& v); + + + //! @return a zero Twist : Twist(Vector::Zero(),Vector::Zero()) + static inline Twist Zero(); + + //! Reverses the sign of the twist + inline void ReverseSign(); + + //! Changes the reference point of the twist. + //! The vector v_base_AB is expressed in the same base as the twist + //! The vector v_base_AB is a vector from the old point to + //! the new point. + //! + //! Complexity : 6M+6A + inline Twist RefPoint(const Vector& v_base_AB) const; + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Twist& a,const Twist& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const Twist& a,const Twist& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const Twist& a,const Twist& b); + +// = Friends + friend class Rotation; + friend class Frame; + +}; + +/** + * \brief represents both translational and rotational acceleration. + * + * This class represents an acceleration twist. A acceleration twist is + * the combination of translational + * acceleration and rotational acceleration applied at one point. +*/ +/* +class AccelerationTwist { +public: + Vector trans; //!< The translational acceleration of that point + Vector rot; //!< The rotational acceleration of that point. +public: + + //! The default constructor initialises to Zero via the constructor of Vector. + AccelerationTwist():trans(),rot() {}; + + AccelerationTwist(const Vector& _trans,const Vector& _rot):trans(_trans),rot(_rot) {}; + + inline AccelerationTwist& operator-=(const AccelerationTwist& arg); + inline AccelerationTwist& operator+=(const AccelerationTwist& arg); + //! index-based access to components, first vel(0..2), then rot(3..5) + inline double& operator()(int i); + + //! index-based access to components, first vel(0..2), then rot(3..5) + //! For use with a const AccelerationTwist + inline double operator()(int i) const; + + double operator[] ( int index ) const + { + return this->operator() ( index ); + } + + double& operator[] ( int index ) + { + return this->operator() ( index ); + } + + inline friend AccelerationTwist operator*(const AccelerationTwist& lhs,double rhs); + inline friend AccelerationTwist operator*(double lhs,const AccelerationTwist& rhs); + inline friend AccelerationTwist operator/(const AccelerationTwist& lhs,double rhs); + inline friend AccelerationTwist operator+(const AccelerationTwist& lhs,const AccelerationTwist& rhs); + inline friend AccelerationTwist operator-(const AccelerationTwist& lhs,const AccelerationTwist& rhs); + inline friend AccelerationTwist operator-(const AccelerationTwist& arg); + //inline friend double dot(const AccelerationTwist& lhs,const Wrench& rhs); + //inline friend double dot(const Wrench& rhs,const AccelerationTwist& lhs); + inline friend void SetToZero(AccelerationTwist& v); + + + //! @return a zero AccelerationTwist : AccelerationTwist(Vector::Zero(),Vector::Zero()) + static inline AccelerationTwist Zero(); + + //! Reverses the sign of the AccelerationTwist + inline void ReverseSign(); + + //! Changes the reference point of the AccelerationTwist. + //! The vector v_base_AB is expressed in the same base as the AccelerationTwist + //! The vector v_base_AB is a vector from the old point to + //! the new point. + //! + //! Complexity : 6M+6A + inline AccelerationTwist RefPoint(const Vector& v_base_AB) const; + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b); + +// = Friends + friend class Rotation; + friend class Frame; + +}; +*/ +/** + * \brief represents the combination of a force and a torque. + * + * This class represents a Wrench. A Wrench is the force and torque applied at a point + */ +class Wrench +{ +public: + Vector force; //!< Force that is applied at the origin of the current ref frame + Vector torque; //!< Torque that is applied at the origin of the current ref frame +public: + + //! Does initialise force and torque to zero via the underlying constructor of Vector + Wrench():force(),torque() {}; + Wrench(const Vector& _force,const Vector& _torque):force(_force),torque(_torque) {}; + +// = Operators + inline Wrench& operator-=(const Wrench& arg); + inline Wrench& operator+=(const Wrench& arg); + + //! index-based access to components, first force(0..2), then torque(3..5) + inline double& operator()(int i); + + //! index-based access to components, first force(0..2), then torque(3..5) + //! for use with a const Wrench + inline double operator()(int i) const; + + double operator[] ( int index ) const + { + return this->operator() ( index ); + } + + double& operator[] ( int index ) + { + return this->operator() ( index ); + } + + //! Scalar multiplication + inline friend Wrench operator*(const Wrench& lhs,double rhs); + //! Scalar multiplication + inline friend Wrench operator*(double lhs,const Wrench& rhs); + //! Scalar division + inline friend Wrench operator/(const Wrench& lhs,double rhs); + + inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs); + inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs); + + //! An unary - operator + inline friend Wrench operator-(const Wrench& arg); + + //! Sets the Wrench to Zero, to have a uniform function that sets an object or + //! double to zero. + inline friend void SetToZero(Wrench& v); + + //! @return a zero Wrench + static inline Wrench Zero(); + + //! Reverses the sign of the current Wrench + inline void ReverseSign(); + + //! Changes the reference point of the wrench. + //! The vector v_base_AB is expressed in the same base as the twist + //! The vector v_base_AB is a vector from the old point to + //! the new point. + //! + //! Complexity : 6M+6A + inline Wrench RefPoint(const Vector& v_base_AB) const; + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon); + + //! The literal equality operator==(), also identical. + inline friend bool operator==(const Wrench& a,const Wrench& b); + //! The literal inequality operator!=(). + inline friend bool operator!=(const Wrench& a,const Wrench& b); + + friend class Rotation; + friend class Frame; + + +}; + + +//! 2D version of Vector +class Vector2 +{ + double data[2]; +public: + //! Does not initialise to Zero(). + Vector2() {data[0]=data[1] = 0.0;} + inline Vector2(double x,double y); + inline Vector2(const Vector2& arg); + inline Vector2(double* xyz); + inline Vector2(float* xyz); + + inline Vector2& operator = ( const Vector2& arg); + + //! Access to elements, range checked when NDEBUG is not set, from 0..1 + inline double operator()(int index) const; + + //! Access to elements, range checked when NDEBUG is not set, from 0..1 + inline double& operator() (int index); + + //! store vector components in array + inline void GetValue(double* xy) const; + + inline void ReverseSign(); + inline Vector2& operator-=(const Vector2& arg); + inline Vector2& operator +=(const Vector2& arg); + + + inline friend Vector2 operator*(const Vector2& lhs,double rhs); + inline friend Vector2 operator*(double lhs,const Vector2& rhs); + inline friend Vector2 operator/(const Vector2& lhs,double rhs); + inline friend Vector2 operator+(const Vector2& lhs,const Vector2& rhs); + inline friend Vector2 operator-(const Vector2& lhs,const Vector2& rhs); + inline friend Vector2 operator*(const Vector2& lhs,const Vector2& rhs); + inline friend Vector2 operator-(const Vector2& arg); + inline friend void SetToZero(Vector2& v); + + //! @return a zero 2D vector. + inline static Vector2 Zero(); + + /** Normalizes this vector and returns it norm + * makes v a unitvector and returns the norm of v. + * if v is smaller than eps, Vector(1,0,0) is returned with norm 0. + * if this is not good, check the return value of this method. + */ + double Normalize(double eps=epsilon); + + //! @return the norm of the vector + inline double Norm() const; + + //! projects v in its XY plane, and sets *this to these values + inline void Set3DXY(const Vector& v); + + //! projects v in its YZ plane, and sets *this to these values + inline void Set3DYZ(const Vector& v); + + //! projects v in its ZX plane, and sets *this to these values + inline void Set3DZX(const Vector& v); + + //! projects v_someframe in the XY plane of F_someframe_XY, + //! and sets *this to these values + //! expressed wrt someframe. + inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe); + + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon); + + friend class Rotation2; +}; + + +//! A 2D Rotation class, for conventions see Rotation. For further documentation +//! of the methods see Rotation class. +class Rotation2 +{ + double s,c; + //! c,s represent cos(angle), sin(angle), this also represents first col. of rot matrix + //! from outside, this class behaves as if it would store the complete 2x2 matrix. +public: + //! Default constructor does NOT initialise to Zero(). + Rotation2() {c=1.0;s=0.0;} + + explicit Rotation2(double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {} + + Rotation2(double ca,double sa):s(sa),c(ca){} + + inline Rotation2& operator=(const Rotation2& arg); + inline Vector2 operator*(const Vector2& v) const; + //! Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set + inline double operator() (int i,int j) const; + + inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs); + + inline void SetInverse(); + inline Rotation2 Inverse() const; + inline Vector2 Inverse(const Vector2& v) const; + + inline void SetIdentity(); + inline static Rotation2 Identity(); + + + //! The SetRot.. functions set the value of *this to the appropriate rotation matrix. + inline void SetRot(double angle); + + //! The Rot... static functions give the value of the appropriate rotation matrix bac + inline static Rotation2 Rot(double angle); + + //! Gets the angle (in radians) + inline double GetRot() const; + + //! do not use operator == because the definition of Equal(.,.) is slightly + //! different. It compares whether the 2 arguments are equal in an eps-interval + inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon); +}; + +//! A 2D frame class, for further documentation see the Frames class +//! for methods with unchanged semantics. +class Frame2 + { +public: + Vector2 p; //!< origine of the Frame + Rotation2 M; //!< Orientation of the Frame + +public: + + inline Frame2(const Rotation2& R,const Vector2& V); + explicit inline Frame2(const Vector2& V); + explicit inline Frame2(const Rotation2& R); + inline Frame2(void); + inline Frame2(const Frame2& arg); + inline void Make4x4(double* d); + + //! Treats a frame as a 3x3 matrix and returns element i,j + //! Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set + inline double operator()(int i,int j); + + //! Treats a frame as a 4x4 matrix and returns element i,j + //! Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set + inline double operator() (int i,int j) const; + + inline void SetInverse(); + inline Frame2 Inverse() const; + inline Vector2 Inverse(const Vector2& arg) const; + inline Frame2& operator = (const Frame2& arg); + inline Vector2 operator * (const Vector2& arg); + inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs); + inline void SetIdentity(); + inline void Integrate(const Twist& t_this,double frequency); + inline static Frame2 Identity() { + Frame2 tmp; + tmp.SetIdentity(); + return tmp; + } + inline friend bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon); +}; + +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); +#ifdef KDL_INLINE +// #include "vector.inl" +// #include "wrench.inl" + //#include "rotation.inl" + //#include "frame.inl" + //#include "twist.inl" + //#include "vector2.inl" + //#include "rotation2.inl" + //#include "frame2.inl" +#include "frames.inl" +#endif + + + +} + + +#endif diff --git a/intern/itasc/kdl/frames.inl b/intern/itasc/kdl/frames.inl new file mode 100644 index 00000000000..9a176070171 --- /dev/null +++ b/intern/itasc/kdl/frames.inl @@ -0,0 +1,1390 @@ +/*************************************************************************** + frames.inl - description + ------------------------- + begin : June 2006 + copyright : (C) 2006 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 * + * * + ***************************************************************************/ + + +IMETHOD Vector::Vector(const Vector & arg) +{ + data[0] = arg.data[0]; + data[1] = arg.data[1]; + data[2] = arg.data[2]; +} + +IMETHOD Vector::Vector(double x,double y, double z) +{ + data[0]=x;data[1]=y;data[2]=z; +} + +IMETHOD Vector::Vector(double* xyz) +{ + data[0]=xyz[0];data[1]=xyz[1];data[2]=xyz[2]; +} + +IMETHOD Vector::Vector(float* xyz) +{ + data[0]=xyz[0];data[1]=xyz[1];data[2]=xyz[2]; +} + +IMETHOD void Vector::GetValue(double* xyz) const +{ + xyz[0]=data[0];xyz[1]=data[1];xyz[2]=data[2]; +} + + +IMETHOD Vector& Vector::operator =(const Vector & arg) +{ + data[0] = arg.data[0]; + data[1] = arg.data[1]; + data[2] = arg.data[2]; + return *this; +} + +IMETHOD Vector operator +(const Vector & lhs,const Vector& rhs) +{ + Vector tmp; + tmp.data[0] = lhs.data[0]+rhs.data[0]; + tmp.data[1] = lhs.data[1]+rhs.data[1]; + tmp.data[2] = lhs.data[2]+rhs.data[2]; + return tmp; +} + +IMETHOD Vector operator -(const Vector & lhs,const Vector& rhs) +{ + Vector tmp; + tmp.data[0] = lhs.data[0]-rhs.data[0]; + tmp.data[1] = lhs.data[1]-rhs.data[1]; + tmp.data[2] = lhs.data[2]-rhs.data[2]; + return tmp; +} + +IMETHOD double Vector::x() const { return data[0]; } +IMETHOD double Vector::y() const { return data[1]; } +IMETHOD double Vector::z() const { return data[2]; } + +IMETHOD void Vector::x( double _x ) { data[0] = _x; } +IMETHOD void Vector::y( double _y ) { data[1] = _y; } +IMETHOD void Vector::z( double _z ) { data[2] = _z; } + +Vector operator *(const Vector& lhs,double rhs) +{ + Vector tmp; + tmp.data[0] = lhs.data[0]*rhs; + tmp.data[1] = lhs.data[1]*rhs; + tmp.data[2] = lhs.data[2]*rhs; + return tmp; +} + +Vector operator *(double lhs,const Vector& rhs) +{ + Vector tmp; + tmp.data[0] = lhs*rhs.data[0]; + tmp.data[1] = lhs*rhs.data[1]; + tmp.data[2] = lhs*rhs.data[2]; + return tmp; +} + +Vector operator /(const Vector& lhs,double rhs) +{ + Vector tmp; + tmp.data[0] = lhs.data[0]/rhs; + tmp.data[1] = lhs.data[1]/rhs; + tmp.data[2] = lhs.data[2]/rhs; + return tmp; +} + +Vector operator *(const Vector & lhs,const Vector& rhs) +// Complexity : 6M+3A +{ + Vector tmp; + tmp.data[0] = lhs.data[1]*rhs.data[2]-lhs.data[2]*rhs.data[1]; + tmp.data[1] = lhs.data[2]*rhs.data[0]-lhs.data[0]*rhs.data[2]; + tmp.data[2] = lhs.data[0]*rhs.data[1]-lhs.data[1]*rhs.data[0]; + return tmp; +} + +Vector& Vector::operator +=(const Vector & arg) +// Complexity : 3A +{ + data[0]+=arg.data[0]; + data[1]+=arg.data[1]; + data[2]+=arg.data[2]; + return *this; +} + +Vector& Vector::operator -=(const Vector & arg) +// Complexity : 3A +{ + data[0]-=arg.data[0]; + data[1]-=arg.data[1]; + data[2]-=arg.data[2]; + return *this; +} + +Vector Vector::Zero() +{ + return Vector(0,0,0); +} + +double Vector::operator()(int index) const { + FRAMES_CHECKI((0<=index)&&(index<=2)); + return data[index]; +} + +double& Vector::operator () (int index) +{ + FRAMES_CHECKI((0<=index)&&(index<=2)); + return data[index]; +} + +IMETHOD Vector Normalize(const Vector& a, double eps) +{ + double l=a.Norm(); + return (l<eps) ? Vector(0.0,0.0,0.0) : a/l; +} + +Wrench Frame::operator * (const Wrench& arg) const +// Complexity : 24M+18A +{ + Wrench tmp; + tmp.force = M*arg.force; + tmp.torque = M*arg.torque + p*tmp.force; + return tmp; +} + +Wrench Frame::Inverse(const Wrench& arg) const +{ + Wrench tmp; + tmp.force = M.Inverse(arg.force); + tmp.torque = M.Inverse(arg.torque-p*arg.force); + return tmp; +} + + + +Wrench Rotation::Inverse(const Wrench& arg) const +{ + return Wrench(Inverse(arg.force),Inverse(arg.torque)); +} + +Twist Rotation::Inverse(const Twist& arg) const +{ + return Twist(Inverse(arg.vel),Inverse(arg.rot)); +} + +Wrench Wrench::Zero() +{ + return Wrench(Vector::Zero(),Vector::Zero()); +} + + +void Wrench::ReverseSign() +{ + torque.ReverseSign(); + force.ReverseSign(); +} + +Wrench Wrench::RefPoint(const Vector& v_base_AB) const + // Changes the reference point of the Wrench. + // The vector v_base_AB is expressed in the same base as the twist + // The vector v_base_AB is a vector from the old point to + // the new point. +{ + return Wrench(this->force, + this->torque+this->force*v_base_AB + ); +} + + +Wrench& Wrench::operator-=(const Wrench& arg) +{ + torque-=arg.torque; + force -=arg.force; + return *this; +} + +Wrench& Wrench::operator+=(const Wrench& arg) +{ + torque+=arg.torque; + force +=arg.force; + return *this; +} + +double& Wrench::operator()(int i) +{ + // assert((0<=i)&&(i<6)); done by underlying routines + if (i<3) + return force(i); + else + return torque(i-3); +} + +double Wrench::operator()(int i) const +{ + // assert((0<=i)&&(i<6)); done by underlying routines + if (i<3) + return force(i); + else + return torque(i-3); +} + + +Wrench operator*(const Wrench& lhs,double rhs) +{ + return Wrench(lhs.force*rhs,lhs.torque*rhs); +} + +Wrench operator*(double lhs,const Wrench& rhs) +{ + return Wrench(lhs*rhs.force,lhs*rhs.torque); +} + +Wrench operator/(const Wrench& lhs,double rhs) +{ + return Wrench(lhs.force/rhs,lhs.torque/rhs); +} + +// addition of Wrench's +Wrench operator+(const Wrench& lhs,const Wrench& rhs) +{ + return Wrench(lhs.force+rhs.force,lhs.torque+rhs.torque); +} + +Wrench operator-(const Wrench& lhs,const Wrench& rhs) +{ + return Wrench(lhs.force-rhs.force,lhs.torque-rhs.torque); +} + +// unary - +Wrench operator-(const Wrench& arg) +{ + return Wrench(-arg.force,-arg.torque); +} + +Twist Frame::operator * (const Twist& arg) const +// Complexity : 24M+18A +{ + Twist tmp; + tmp.rot = M*arg.rot; + tmp.vel = M*arg.vel+p*tmp.rot; + return tmp; +} +Twist Frame::Inverse(const Twist& arg) const +{ + Twist tmp; + tmp.rot = M.Inverse(arg.rot); + tmp.vel = M.Inverse(arg.vel-p*arg.rot); + return tmp; +} + +Twist Twist::Zero() +{ + return Twist(Vector::Zero(),Vector::Zero()); +} + + +void Twist::ReverseSign() +{ + vel.ReverseSign(); + rot.ReverseSign(); +} + +Twist Twist::RefPoint(const Vector& v_base_AB) const + // Changes the reference point of the twist. + // The vector v_base_AB is expressed in the same base as the twist + // The vector v_base_AB is a vector from the old point to + // the new point. + // Complexity : 6M+6A +{ + return Twist(this->vel+this->rot*v_base_AB,this->rot); +} + +Twist& Twist::operator-=(const Twist& arg) +{ + vel-=arg.vel; + rot -=arg.rot; + return *this; +} + +Twist& Twist::operator+=(const Twist& arg) +{ + vel+=arg.vel; + rot +=arg.rot; + return *this; +} + +double& Twist::operator()(int i) +{ + // assert((0<=i)&&(i<6)); done by underlying routines + if (i<3) + return vel(i); + else + return rot(i-3); +} + +double Twist::operator()(int i) const +{ + // assert((0<=i)&&(i<6)); done by underlying routines + if (i<3) + return vel(i); + else + return rot(i-3); +} + + +Twist operator*(const Twist& lhs,double rhs) +{ + return Twist(lhs.vel*rhs,lhs.rot*rhs); +} + +Twist operator*(double lhs,const Twist& rhs) +{ + return Twist(lhs*rhs.vel,lhs*rhs.rot); +} + +Twist operator/(const Twist& lhs,double rhs) +{ + return Twist(lhs.vel/rhs,lhs.rot/rhs); +} + +// addition of Twist's +Twist operator+(const Twist& lhs,const Twist& rhs) +{ + return Twist(lhs.vel+rhs.vel,lhs.rot+rhs.rot); +} + +Twist operator-(const Twist& lhs,const Twist& rhs) +{ + return Twist(lhs.vel-rhs.vel,lhs.rot-rhs.rot); +} + +// unary - +Twist operator-(const Twist& arg) +{ + return Twist(-arg.vel,-arg.rot); +} + +Frame::Frame(const Rotation & R) +{ + M=R; + p=Vector::Zero(); +} + +Frame::Frame(const Vector & V) +{ + M = Rotation::Identity(); + p = V; +} + +Frame::Frame(const Rotation & R, const Vector & V) +{ + M = R; + p = V; +} + + Frame operator *(const Frame& lhs,const Frame& rhs) +// Complexity : 36M+36A +{ + return Frame(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} + +Vector Frame::operator *(const Vector & arg) const +{ + return M*arg+p; +} + +Vector Frame::Inverse(const Vector& arg) const +{ + return M.Inverse(arg-p); +} + +Frame Frame::Inverse() const +{ + return Frame(M.Inverse(),-M.Inverse(p)); +} + + +Frame& Frame::operator =(const Frame & arg) +{ + M = arg.M; + p = arg.p; + return *this; +} + +Frame::Frame(const Frame & arg) : + p(arg.p),M(arg.M) +{} + + +void Vector::ReverseSign() +{ + data[0] = -data[0]; + data[1] = -data[1]; + data[2] = -data[2]; +} + + + +Vector operator-(const Vector & arg) +{ + Vector tmp; + tmp.data[0]=-arg.data[0]; + tmp.data[1]=-arg.data[1]; + tmp.data[2]=-arg.data[2]; + return tmp; +} + +void Vector::Set2DXY(const Vector2& v) +// a 3D vector where the 2D vector v is put in the XY plane +{ + data[0]=v(0); + data[1]=v(1); + data[2]=0; + +} +void Vector::Set2DYZ(const Vector2& v) +// a 3D vector where the 2D vector v is put in the YZ plane +{ + data[1]=v(0); + data[2]=v(1); + data[0]=0; + +} + +void Vector::Set2DZX(const Vector2& v) +// a 3D vector where the 2D vector v is put in the ZX plane +{ + data[2]=v(0); + data[0]=v(1); + data[1]=0; + +} + + + + + +double& Rotation::operator()(int i,int j) { + FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2)); + return data[i*3+j]; +} + +double Rotation::operator()(int i,int j) const { + FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2)); + return data[i*3+j]; +} + +Rotation::Rotation( double Xx,double Yx,double Zx, + double Xy,double Yy,double Zy, + double Xz,double Yz,double Zz) +{ + data[0] = Xx;data[1]=Yx;data[2]=Zx; + data[3] = Xy;data[4]=Yy;data[5]=Zy; + data[6] = Xz;data[7]=Yz;data[8]=Zz; +} + + +Rotation::Rotation(const Vector& x,const Vector& y,const Vector& z) +{ + data[0] = x.data[0];data[3] = x.data[1];data[6] = x.data[2]; + data[1] = y.data[0];data[4] = y.data[1];data[7] = y.data[2]; + data[2] = z.data[0];data[5] = z.data[1];data[8] = z.data[2]; +} + +Rotation& Rotation::operator=(const Rotation& arg) { + int count=9; + while (count--) data[count] = arg.data[count]; + return *this; +} + +Vector Rotation::operator*(const Vector& v) const { +// Complexity : 9M+6A + return Vector( + data[0]*v.data[0] + data[1]*v.data[1] + data[2]*v.data[2], + data[3]*v.data[0] + data[4]*v.data[1] + data[5]*v.data[2], + data[6]*v.data[0] + data[7]*v.data[1] + data[8]*v.data[2] + ); +} + +Twist Rotation::operator * (const Twist& arg) const + // Transformation of the base to which the twist is expressed. + // look at Frame*Twist for a transformation that also transforms + // the velocity reference point. + // Complexity : 18M+12A +{ + return Twist((*this)*arg.vel,(*this)*arg.rot); +} + +Wrench Rotation::operator * (const Wrench& arg) const + // Transformation of the base to which the wrench is expressed. + // look at Frame*Twist for a transformation that also transforms + // the force reference point. +{ + return Wrench((*this)*arg.force,(*this)*arg.torque); +} + +Rotation Rotation::Identity() { + return Rotation(1,0,0,0,1,0,0,0,1); +} +// *this = *this * ROT(X,angle) +void Rotation::DoRotX(double angle) +{ + double cs = cos(angle); + double sn = sin(angle); + double x1,x2,x3; + x1 = cs* (*this)(0,1) + sn* (*this)(0,2); + x2 = cs* (*this)(1,1) + sn* (*this)(1,2); + x3 = cs* (*this)(2,1) + sn* (*this)(2,2); + (*this)(0,2) = -sn* (*this)(0,1) + cs* (*this)(0,2); + (*this)(1,2) = -sn* (*this)(1,1) + cs* (*this)(1,2); + (*this)(2,2) = -sn* (*this)(2,1) + cs* (*this)(2,2); + (*this)(0,1) = x1; + (*this)(1,1) = x2; + (*this)(2,1) = x3; +} + +void Rotation::DoRotY(double angle) +{ + double cs = cos(angle); + double sn = sin(angle); + double x1,x2,x3; + x1 = cs* (*this)(0,0) - sn* (*this)(0,2); + x2 = cs* (*this)(1,0) - sn* (*this)(1,2); + x3 = cs* (*this)(2,0) - sn* (*this)(2,2); + (*this)(0,2) = sn* (*this)(0,0) + cs* (*this)(0,2); + (*this)(1,2) = sn* (*this)(1,0) + cs* (*this)(1,2); + (*this)(2,2) = sn* (*this)(2,0) + cs* (*this)(2,2); + (*this)(0,0) = x1; + (*this)(1,0) = x2; + (*this)(2,0) = x3; +} + +void Rotation::DoRotZ(double angle) +{ + double cs = cos(angle); + double sn = sin(angle); + double x1,x2,x3; + x1 = cs* (*this)(0,0) + sn* (*this)(0,1); + x2 = cs* (*this)(1,0) + sn* (*this)(1,1); + x3 = cs* (*this)(2,0) + sn* (*this)(2,1); + (*this)(0,1) = -sn* (*this)(0,0) + cs* (*this)(0,1); + (*this)(1,1) = -sn* (*this)(1,0) + cs* (*this)(1,1); + (*this)(2,1) = -sn* (*this)(2,0) + cs* (*this)(2,1); + (*this)(0,0) = x1; + (*this)(1,0) = x2; + (*this)(2,0) = x3; +} + + +Rotation Rotation::RotX(double angle) { + double cs=cos(angle); + double sn=sin(angle); + return Rotation(1,0,0,0,cs,-sn,0,sn,cs); +} +Rotation Rotation::RotY(double angle) { + double cs=cos(angle); + double sn=sin(angle); + return Rotation(cs,0,sn,0,1,0,-sn,0,cs); +} +Rotation Rotation::RotZ(double angle) { + double cs=cos(angle); + double sn=sin(angle); + return Rotation(cs,-sn,0,sn,cs,0,0,0,1); +} + + + + +void Frame::Integrate(const Twist& t_this,double samplefrequency) +{ + double n = t_this.rot.Norm()/samplefrequency; + if (n<epsilon) { + p += M*(t_this.vel/samplefrequency); + } else { + (*this) = (*this) * + Frame ( Rotation::Rot( t_this.rot, n ), + t_this.vel/samplefrequency + ); + } +} + +Rotation Rotation::Inverse() const +{ + Rotation tmp(*this); + tmp.SetInverse(); + return tmp; +} + +Vector Rotation::Inverse(const Vector& v) const { + return Vector( + data[0]*v.data[0] + data[3]*v.data[1] + data[6]*v.data[2], + data[1]*v.data[0] + data[4]*v.data[1] + data[7]*v.data[2], + data[2]*v.data[0] + data[5]*v.data[1] + data[8]*v.data[2] + ); +} + +void Rotation::setValue(float* oglmat) +{ + data[0] = *oglmat++; data[3] = *oglmat++; data[6] = *oglmat++; oglmat++; + data[1] = *oglmat++; data[4] = *oglmat++; data[7] = *oglmat++; oglmat++; + data[2] = *oglmat++; data[5] = *oglmat++; data[8] = *oglmat; + Ortho(); +} + +void Rotation::getValue(float* oglmat) const +{ + *oglmat++ = (float)data[0]; *oglmat++ = (float)data[3]; *oglmat++ = (float)data[6]; *oglmat++ = 0.f; + *oglmat++ = (float)data[1]; *oglmat++ = (float)data[4]; *oglmat++ = (float)data[7]; *oglmat++ = 0.f; + *oglmat++ = (float)data[2]; *oglmat++ = (float)data[5]; *oglmat++ = (float)data[8]; *oglmat++ = 0.f; + *oglmat++ = 0.f; *oglmat++ = 0.f; *oglmat++ = 0.f; *oglmat = 1.f; +} + +void Rotation::SetInverse() +{ + double tmp; + tmp = data[1];data[1]=data[3];data[3]=tmp; + tmp = data[2];data[2]=data[6];data[6]=tmp; + tmp = data[5];data[5]=data[7];data[7]=tmp; +} + + + + + + + +double Frame::operator()(int i,int j) { + FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3)); + if (i==3) { + if (j==3) + return 1.0; + else + return 0.0; + } else { + if (j==3) + return p(i); + else + return M(i,j); + + } +} + +double Frame::operator()(int i,int j) const { + FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3)); + if (i==3) { + if (j==3) + return 1; + else + return 0; + } else { + if (j==3) + return p(i); + else + return M(i,j); + + } +} + + +Frame Frame::Identity() { + return Frame(Rotation::Identity(),Vector::Zero()); +} + + +void Frame::setValue(float* oglmat) +{ + M.setValue(oglmat); + p.data[0] = oglmat[12]; + p.data[1] = oglmat[13]; + p.data[2] = oglmat[14]; +} + +void Frame::getValue(float* oglmat) const +{ + M.getValue(oglmat); + oglmat[12] = (float)p.data[0]; + oglmat[13] = (float)p.data[1]; + oglmat[14] = (float)p.data[2]; +} + +void Vector::Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY) +// a 3D vector where the 2D vector v is put in the XY plane of the frame +// F_someframe_XY. +{ +Vector tmp_XY; +tmp_XY.Set2DXY(v_XY); +tmp_XY = F_someframe_XY*(tmp_XY); +} + + + + + + + + + +//============ 2 dimensional version of the frames objects ============= +IMETHOD Vector2::Vector2(const Vector2 & arg) +{ + data[0] = arg.data[0]; + data[1] = arg.data[1]; +} + +IMETHOD Vector2::Vector2(double x,double y) +{ + data[0]=x;data[1]=y; +} + +IMETHOD Vector2::Vector2(double* xy) +{ + data[0]=xy[0];data[1]=xy[1]; +} + +IMETHOD Vector2::Vector2(float* xy) +{ + data[0]=xy[0];data[1]=xy[1]; +} + +IMETHOD Vector2& Vector2::operator =(const Vector2 & arg) +{ + data[0] = arg.data[0]; + data[1] = arg.data[1]; + return *this; +} + +IMETHOD void Vector2::GetValue(double* xy) const +{ + xy[0]=data[0];xy[1]=data[1]; +} + +IMETHOD Vector2 operator +(const Vector2 & lhs,const Vector2& rhs) +{ + return Vector2(lhs.data[0]+rhs.data[0],lhs.data[1]+rhs.data[1]); +} + +IMETHOD Vector2 operator -(const Vector2 & lhs,const Vector2& rhs) +{ + return Vector2(lhs.data[0]-rhs.data[0],lhs.data[1]-rhs.data[1]); +} + +IMETHOD Vector2 operator *(const Vector2& lhs,double rhs) +{ + return Vector2(lhs.data[0]*rhs,lhs.data[1]*rhs); +} + +IMETHOD Vector2 operator *(double lhs,const Vector2& rhs) +{ + return Vector2(lhs*rhs.data[0],lhs*rhs.data[1]); +} + +IMETHOD Vector2 operator /(const Vector2& lhs,double rhs) +{ + return Vector2(lhs.data[0]/rhs,lhs.data[1]/rhs); +} + +IMETHOD Vector2& Vector2::operator +=(const Vector2 & arg) +{ + data[0]+=arg.data[0]; + data[1]+=arg.data[1]; + return *this; +} + +IMETHOD Vector2& Vector2::operator -=(const Vector2 & arg) +{ + data[0]-=arg.data[0]; + data[1]-=arg.data[1]; + return *this; +} + +IMETHOD Vector2 Vector2::Zero() { + return Vector2(0,0); +} + +IMETHOD double Vector2::operator()(int index) const { + FRAMES_CHECKI((0<=index)&&(index<=1)); + return data[index]; +} + +IMETHOD double& Vector2::operator () (int index) +{ + FRAMES_CHECKI((0<=index)&&(index<=1)); + return data[index]; +} +IMETHOD void Vector2::ReverseSign() +{ + data[0] = -data[0]; + data[1] = -data[1]; +} + + +IMETHOD Vector2 operator-(const Vector2 & arg) +{ + return Vector2(-arg.data[0],-arg.data[1]); +} + + +IMETHOD void Vector2::Set3DXY(const Vector& v) +// projects v in its XY plane, and sets *this to these values +{ + data[0]=v(0); + data[1]=v(1); +} +IMETHOD void Vector2::Set3DYZ(const Vector& v) +// projects v in its XY plane, and sets *this to these values +{ + data[0]=v(1); + data[1]=v(2); +} +IMETHOD void Vector2::Set3DZX(const Vector& v) +// projects v in its XY plane, and and sets *this to these values +{ + data[0]=v(2); + data[1]=v(0); +} + +IMETHOD void Vector2::Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe) +// projects v in the XY plane of F_someframe_XY, and sets *this to these values +// expressed wrt someframe. +{ + Vector tmp = F_someframe_XY.Inverse(v_someframe); + data[0]=tmp(0); + data[1]=tmp(1); +} + + + +IMETHOD Rotation2& Rotation2::operator=(const Rotation2& arg) { + c=arg.c;s=arg.s; + return *this; +} + +IMETHOD Vector2 Rotation2::operator*(const Vector2& v) const { + return Vector2(v.data[0]*c-v.data[1]*s,v.data[0]*s+v.data[1]*c); +} + +IMETHOD double Rotation2::operator()(int i,int j) const { + FRAMES_CHECKI((0<=i)&&(i<=1)&&(0<=j)&&(j<=1)); + if (i==j) return c; + if (i==0) + return s; + else + return -s; +} + + +IMETHOD Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs) { + return Rotation2(lhs.c*rhs.c-lhs.s*rhs.s,lhs.s*rhs.c+lhs.c*rhs.s); +} + +IMETHOD void Rotation2::SetInverse() { + s=-s; +} + +IMETHOD Rotation2 Rotation2::Inverse() const { + return Rotation2(c,-s); +} + +IMETHOD Vector2 Rotation2::Inverse(const Vector2& v) const { + return Vector2(v.data[0]*c+v.data[1]*s,-v.data[0]*s+v.data[1]*c); +} + +IMETHOD Rotation2 Rotation2::Identity() { + return Rotation2(1,0); +} + +IMETHOD void Rotation2::SetIdentity() +{ + c = 1; + s = 0; +} + +IMETHOD void Rotation2::SetRot(double angle) { + c=cos(angle);s=sin(angle); +} + +IMETHOD Rotation2 Rotation2::Rot(double angle) { + return Rotation2(cos(angle),sin(angle)); +} + +IMETHOD double Rotation2::GetRot() const { + return atan2(s,c); +} + + +IMETHOD Frame2::Frame2() { +} + +IMETHOD Frame2::Frame2(const Rotation2 & R) +{ + M=R; + p=Vector2::Zero(); +} + +IMETHOD Frame2::Frame2(const Vector2 & V) +{ + M = Rotation2::Identity(); + p = V; +} + +IMETHOD Frame2::Frame2(const Rotation2 & R, const Vector2 & V) +{ + M = R; + p = V; +} + +IMETHOD Frame2 operator *(const Frame2& lhs,const Frame2& rhs) +{ + return Frame2(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} + +IMETHOD Vector2 Frame2::operator *(const Vector2 & arg) +{ + return M*arg+p; +} + +IMETHOD Vector2 Frame2::Inverse(const Vector2& arg) const +{ + return M.Inverse(arg-p); +} + +IMETHOD void Frame2::SetIdentity() +{ + M.SetIdentity(); + p = Vector2::Zero(); +} + +IMETHOD void Frame2::SetInverse() +{ + M.SetInverse(); + p = M*p; + p.ReverseSign(); +} + + +IMETHOD Frame2 Frame2::Inverse() const +{ + Frame2 tmp(*this); + tmp.SetInverse(); + return tmp; +} + +IMETHOD Frame2& Frame2::operator =(const Frame2 & arg) +{ + M = arg.M; + p = arg.p; + return *this; +} + +IMETHOD Frame2::Frame2(const Frame2 & arg) : + p(arg.p), M(arg.M) +{} + + +IMETHOD double Frame2::operator()(int i,int j) { + FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2)); + if (i==2) { + if (j==2) + return 1; + else + return 0; + } else { + if (j==2) + return p(i); + else + return M(i,j); + + } +} + +IMETHOD double Frame2::operator()(int i,int j) const { + FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2)); + if (i==2) { + if (j==2) + return 1; + else + return 0; + } else { + if (j==2) + return p(i); + else + return M(i,j); + + } +} + +// Scalar products. + +IMETHOD double dot(const Vector& lhs,const Vector& rhs) { + return rhs(0)*lhs(0)+rhs(1)*lhs(1)+rhs(2)*lhs(2); +} + +IMETHOD double dot(const Twist& lhs,const Wrench& rhs) { + return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque); +} + +IMETHOD double dot(const Wrench& rhs,const Twist& lhs) { + return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque); +} + + + + + +// Equality operators + + + +IMETHOD bool Equal(const Vector& a,const Vector& b,double eps) { + return (Equal(a.data[0],b.data[0],eps)&& + Equal(a.data[1],b.data[1],eps)&& + Equal(a.data[2],b.data[2],eps) ); + } + + +IMETHOD bool Equal(const Frame& a,const Frame& b,double eps) { + return (Equal(a.p,b.p,eps)&& + Equal(a.M,b.M,eps) ); +} + +IMETHOD bool Equal(const Wrench& a,const Wrench& b,double eps) { + return (Equal(a.force,b.force,eps)&& + Equal(a.torque,b.torque,eps) ); +} + +IMETHOD bool Equal(const Twist& a,const Twist& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} + +IMETHOD bool Equal(const Vector2& a,const Vector2& b,double eps) { + return (Equal(a.data[0],b.data[0],eps)&& + Equal(a.data[1],b.data[1],eps) ); + } + +IMETHOD bool Equal(const Rotation2& a,const Rotation2& b,double eps) { + return ( Equal(a.c,b.c,eps) && Equal(a.s,b.s,eps) ); +} + +IMETHOD bool Equal(const Frame2& a,const Frame2& b,double eps) { + return (Equal(a.p,b.p,eps)&& + Equal(a.M,b.M,eps) ); +} + +IMETHOD void SetToZero(Vector& v) { + v=Vector::Zero(); +} +IMETHOD void SetToZero(Twist& v) { + SetToZero(v.rot); + SetToZero(v.vel); +} +IMETHOD void SetToZero(Wrench& v) { + SetToZero(v.force); + SetToZero(v.torque); +} + +IMETHOD void SetToZero(Vector2& v) { + v = Vector2::Zero(); +} + + +//////////////////////////////////////////////////////////////// +// The following defines the operations +// diff +// addDelta +// random +// posrandom +// on all the types defined in this library. +// (mostly for uniform integration, differentiation and testing). +// Defined as functions because double is not a class and a method +// would brake uniformity when defined for a double. +//////////////////////////////////////////////////////////////// + + + + + + +/** + * axis_a_b is a rotation vector, its norm is a rotation angle + * axis_a_b rotates the a frame towards the b frame. + * This routine returns the rotation matrix R_a_b + */ +IMETHOD Rotation Rot(const Vector& axis_a_b) { + // The formula is + // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr)) + // can be found by multiplying it with an arbitrary vector p + // and noting that this vector is rotated. + Vector rotvec = axis_a_b; + double angle = rotvec.Normalize(1E-10); + double ct = ::cos(angle); + double st = ::sin(angle); + double vt = 1-ct; + return Rotation( + ct + vt*rotvec(0)*rotvec(0), + -rotvec(2)*st + vt*rotvec(0)*rotvec(1), + rotvec(1)*st + vt*rotvec(0)*rotvec(2), + rotvec(2)*st + vt*rotvec(1)*rotvec(0), + ct + vt*rotvec(1)*rotvec(1), + -rotvec(0)*st + vt*rotvec(1)*rotvec(2), + -rotvec(1)*st + vt*rotvec(2)*rotvec(0), + rotvec(0)*st + vt*rotvec(2)*rotvec(1), + 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), + diff(F_a_b1.M,F_a_b2.M,dt) + ); +} +IMETHOD Twist diff(const Twist& a,const Twist& b,double dt) { + return Twist(diff(a.vel,b.vel,dt),diff(a.rot,b.rot,dt)); +} + +IMETHOD Wrench diff(const Wrench& a,const Wrench& b,double dt) { + return Wrench( + diff(a.force,b.force,dt), + diff(a.torque,b.torque,dt) + ); +} + + +IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt) { + return a+da*dt; +} + +IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt) { + return a*Rot(a.Inverse(da)*dt); +} +IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt) { + return Frame( + addDelta(a.M,da.rot,dt), + addDelta(a.p,da.vel,dt) + ); +} +IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt) { + return Twist(addDelta(a.vel,da.vel,dt),addDelta(a.rot,da.rot,dt)); +} +IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt) { + return Wrench(addDelta(a.force,da.force,dt),addDelta(a.torque,da.torque,dt)); +} + + +/** + * \brief addDelta 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(). + * + * \param a : displacement rotational velocity + * \param da : rotational velocity + * \return displacement rotational velocity + * + * \warning do not confuse displacement rotational velocities and velocities + * \warning do not confuse displacement twist and twist. + * +IMETHOD Vector addDelta_displ(const Vector& a,const Vector&da,double dt) { + return getRot(addDelta(Rot(a),da,dt)); +}*/ + +/** + * \brief addDelta operator for displacement twist. + * + * 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(). + * + * \param a : displacement twist + * \param da : twist + * \return displacement twist + * + * \warning do not confuse displacement rotational velocities and velocities + * \warning do not confuse displacement twist and twist. + * +IMETHOD Twist addDelta_displ(const Twist& a,const Twist&da,double dt) { + return Twist(addDelta(a.vel,da.vel,dt),addDelta_displ(a.rot,da.rot,dt)); +}*/ + + +IMETHOD void random(Vector& a) { + random(a[0]); + random(a[1]); + random(a[2]); +} +IMETHOD void random(Twist& a) { + random(a.rot); + random(a.vel); +} +IMETHOD void random(Wrench& a) { + random(a.torque); + random(a.force); +} + +IMETHOD void random(Rotation& R) { + double alfa; + double beta; + double gamma; + random(alfa); + random(beta); + random(gamma); + R = Rotation::EulerZYX(alfa,beta,gamma); +} + +IMETHOD void random(Frame& F) { + random(F.M); + random(F.p); +} + +IMETHOD void posrandom(Vector& a) { + posrandom(a[0]); + posrandom(a[1]); + posrandom(a[2]); +} +IMETHOD void posrandom(Twist& a) { + posrandom(a.rot); + posrandom(a.vel); +} +IMETHOD void posrandom(Wrench& a) { + posrandom(a.torque); + posrandom(a.force); +} + +IMETHOD void posrandom(Rotation& R) { + double alfa; + double beta; + double gamma; + posrandom(alfa); + posrandom(beta); + posrandom(gamma); + R = Rotation::EulerZYX(alfa,beta,gamma); +} + +IMETHOD void posrandom(Frame& F) { + random(F.M); + random(F.p); +} + + + + +IMETHOD bool operator==(const Frame& a,const Frame& b ) { +#ifdef KDL_USE_EQUAL + return Equal(a,b); +#else + return (a.p == b.p && + a.M == b.M ); +#endif +} + +IMETHOD bool operator!=(const Frame& a,const Frame& b) { + return !operator==(a,b); +} + +IMETHOD bool operator==(const Vector& a,const Vector& b) { +#ifdef KDL_USE_EQUAL + return Equal(a,b); +#else + return (a.data[0]==b.data[0]&& + a.data[1]==b.data[1]&& + a.data[2]==b.data[2] ); +#endif + } + +IMETHOD bool operator!=(const Vector& a,const Vector& b) { + return !operator==(a,b); +} + +IMETHOD bool operator==(const Twist& a,const Twist& b) { +#ifdef KDL_USE_EQUAL + return Equal(a,b); +#else + return (a.rot==b.rot && + a.vel==b.vel ); +#endif +} + +IMETHOD bool operator!=(const Twist& a,const Twist& b) { + return !operator==(a,b); +} + +IMETHOD bool operator==(const Wrench& a,const Wrench& b ) { +#ifdef KDL_USE_EQUAL + return Equal(a,b); +#else + return (a.force==b.force && + a.torque==b.torque ); +#endif +} + +IMETHOD bool operator!=(const Wrench& a,const Wrench& b) { + return !operator==(a,b); +} +IMETHOD bool operator!=(const Rotation& a,const Rotation& b) { + return !operator==(a,b); +} + diff --git a/intern/itasc/kdl/frames_io.cpp b/intern/itasc/kdl/frames_io.cpp new file mode 100644 index 00000000000..0af50bb0e07 --- /dev/null +++ b/intern/itasc/kdl/frames_io.cpp @@ -0,0 +1,310 @@ + +/*************************************************************************** + frames_io.h - description + ------------------------- + begin : June 2006 + copyright : (C) 2006 Erwin Aertbelien + email : firstname.lastname@mech.kuleuven.ac.be + + History (only major changes)( AUTHOR-Description ) : + + *************************************************************************** + * 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 "utilities/error.h" +#include "utilities/error_stack.h" +#include "frames.hpp" +#include "frames_io.hpp" + +#include <stdlib.h> +#include <ctype.h> +#include <string.h> +#include <iostream> + +namespace KDL { + + +std::ostream& operator << (std::ostream& os,const Vector& v) { + os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1) + << "," << std::setw(KDL_FRAME_WIDTH) << v(2) << "]"; + return os; +} + +std::ostream& operator << (std::ostream& os,const Twist& v) { + os << "[" << std::setw(KDL_FRAME_WIDTH) << v.vel(0) + << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(1) + << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(2) + << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(0) + << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(1) + << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(2) + << "]"; + return os; +} + +std::ostream& operator << (std::ostream& os,const Wrench& v) { + os << "[" << std::setw(KDL_FRAME_WIDTH) << v.force(0) + << "," << std::setw(KDL_FRAME_WIDTH) << v.force(1) + << "," << std::setw(KDL_FRAME_WIDTH) << v.force(2) + << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(0) + << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(1) + << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(2) + << "]"; + return os; +} + + +std::ostream& operator << (std::ostream& os,const Rotation& R) { +#ifdef KDL_ROTATION_PROPERTIES_RPY + double r,p,y; + R.GetRPY(r,p,y); + os << "[RPY]"<<endl; + os << "["; + os << std::setw(KDL_FRAME_WIDTH) << r << ","; + os << std::setw(KDL_FRAME_WIDTH) << p << ","; + os << std::setw(KDL_FRAME_WIDTH) << y << "]"; +#else +# ifdef KDL_ROTATION_PROPERTIES_EULER + double z,y,x; + R.GetEulerZYX(z,y,x); + os << "[EULERZYX]"<<endl; + os << "["; + os << std::setw(KDL_FRAME_WIDTH) << z << ","; + os << std::setw(KDL_FRAME_WIDTH) << y << ","; + os << std::setw(KDL_FRAME_WIDTH) << x << "]"; +# else + os << "["; + for (int i=0;i<=2;i++) { + os << std::setw(KDL_FRAME_WIDTH) << R(i,0) << "," << + std::setw(KDL_FRAME_WIDTH) << R(i,1) << "," << + std::setw(KDL_FRAME_WIDTH) << R(i,2); + if (i<2) + os << ";"<< std::endl << " "; + else + os << "]"; + } +# endif +#endif + return os; +} + +std::ostream& operator << (std::ostream& os, const Frame& T) +{ + os << "[" << T.M << std::endl<< T.p << "]"; + return os; +} + +std::ostream& operator << (std::ostream& os,const Vector2& v) { + os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1) + << "]"; + return os; +} + +// Rotation2 gives back an angle in degrees with the << and >> operators. +std::ostream& operator << (std::ostream& os,const Rotation2& R) { + os << "[" << R.GetRot()*rad2deg << "]"; + return os; +} + +std::ostream& operator << (std::ostream& os, const Frame2& T) +{ + os << T.M << T.p; + return os; +} + +std::istream& operator >> (std::istream& is,Vector& v) +{ IOTrace("Stream input Vector (vector or ZERO)"); + char storage[10]; + EatWord(is,"[]",storage,10); + if (strlen(storage)==0) { + Eat(is,'['); + is >> v(0); + Eat(is,','); + is >> v(1); + Eat(is,','); + is >> v(2); + EatEnd(is,']'); + IOTracePop(); + return is; + } + if (strcmp(storage,"ZERO")==0) { + v = Vector::Zero(); + IOTracePop(); + return is; + } + throw Error_Frame_Vector_Unexpected_id(); +} + +std::istream& operator >> (std::istream& is,Twist& v) +{ IOTrace("Stream input Twist"); + Eat(is,'['); + is >> v.vel(0); + Eat(is,','); + is >> v.vel(1); + Eat(is,','); + is >> v.vel(2); + Eat(is,','); + is >> v.rot(0); + Eat(is,','); + is >> v.rot(1); + Eat(is,','); + is >> v.rot(2); + EatEnd(is,']'); + IOTracePop(); + return is; +} + +std::istream& operator >> (std::istream& is,Wrench& v) +{ IOTrace("Stream input Wrench"); + Eat(is,'['); + is >> v.force(0); + Eat(is,','); + is >> v.force(1); + Eat(is,','); + is >> v.force(2); + Eat(is,','); + is >> v.torque(0); + Eat(is,','); + is >> v.torque(1); + Eat(is,','); + is >> v.torque(2); + EatEnd(is,']'); + IOTracePop(); + return is; +} + +std::istream& operator >> (std::istream& is,Rotation& r) +{ IOTrace("Stream input Rotation (Matrix or EULERZYX, EULERZYZ,RPY, ROT, IDENTITY)"); + char storage[10]; + EatWord(is,"[]",storage,10); + if (strlen(storage)==0) { + Eat(is,'['); + for (int i=0;i<3;i++) { + is >> r(i,0); + Eat(is,',') ; + is >> r(i,1); + Eat(is,','); + is >> r(i,2); + if (i<2) + Eat(is,';'); + else + EatEnd(is,']'); + } + IOTracePop(); + return is; + } + Vector v; + if (strcmp(storage,"EULERZYX")==0) { + is >> v; + v=v*deg2rad; + r = Rotation::EulerZYX(v(0),v(1),v(2)); + IOTracePop(); + return is; + } + if (strcmp(storage,"EULERZYZ")==0) { + is >> v; + v=v*deg2rad; + r = Rotation::EulerZYZ(v(0),v(1),v(2)); + IOTracePop(); + return is; + } + if (strcmp(storage,"RPY")==0) { + is >> v; + v=v*deg2rad; + r = Rotation::RPY(v(0),v(1),v(2)); + IOTracePop(); + return is; + } + if (strcmp(storage,"ROT")==0) { + is >> v; + double angle; + Eat(is,'['); + is >> angle; + EatEnd(is,']'); + r = Rotation::Rot(v,angle*deg2rad); + IOTracePop(); + return is; + } + if (strcmp(storage,"IDENTITY")==0) { + r = Rotation::Identity(); + IOTracePop(); + return is; + } + throw Error_Frame_Rotation_Unexpected_id(); + return is; +} + +std::istream& operator >> (std::istream& is,Frame& T) +{ IOTrace("Stream input Frame (Rotation,Vector) or DH[...]"); + char storage[10]; + EatWord(is,"[",storage,10); + if (strlen(storage)==0) { + Eat(is,'['); + is >> T.M; + is >> T.p; + EatEnd(is,']'); + IOTracePop(); + return is; + } + if (strcmp(storage,"DH")==0) { + double a,alpha,d,theta; + Eat(is,'['); + is >> a; + Eat(is,','); + is >> alpha; + Eat(is,','); + is >> d; + Eat(is,','); + is >> theta; + EatEnd(is,']'); + T = Frame::DH(a,alpha*deg2rad,d,theta*deg2rad); + IOTracePop(); + return is; + } + throw Error_Frame_Frame_Unexpected_id(); + return is; +} + +std::istream& operator >> (std::istream& is,Vector2& v) +{ IOTrace("Stream input Vector2"); + Eat(is,'['); + is >> v(0); + Eat(is,','); + is >> v(1); + EatEnd(is,']'); + IOTracePop(); + return is; +} +std::istream& operator >> (std::istream& is,Rotation2& r) +{ IOTrace("Stream input Rotation2"); + Eat(is,'['); + double val; + is >> val; + r.Rot(val*deg2rad); + EatEnd(is,']'); + IOTracePop(); + return is; +} +std::istream& operator >> (std::istream& is,Frame2& T) +{ IOTrace("Stream input Frame2"); + is >> T.M; + is >> T.p; + IOTracePop(); + return is; +} + +} // namespace Frame diff --git a/intern/itasc/kdl/frames_io.hpp b/intern/itasc/kdl/frames_io.hpp new file mode 100644 index 00000000000..a358d27383f --- /dev/null +++ b/intern/itasc/kdl/frames_io.hpp @@ -0,0 +1,114 @@ +/*************************************************************************** + frames_io.h - description + ------------------------- + begin : June 2006 + copyright : (C) 2006 Erwin Aertbelien + email : firstname.lastname@mech.kuleuven.ac.be + + History (only major changes)( AUTHOR-Description ) : + + Ruben Smits - Added output for jacobian and jntarray 06/2007 + + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ +/** +// +// \file +// Defines routines for I/O of Frame and related objects. +// \verbatim +// Spaces, tabs and newlines do not have any importance. +// Comments are allowed C-style,C++-style, make/perl/csh -style +// Description of the I/O : +// Vector : OUTPUT : e.g. [10,20,30] +// INPUT : +// 1) [10,20,30] +// 2) Zero +// Twist : e.g. [1,2,3,4,5,6] +// where [1,2,3] is velocity vector +// where [4,5,6] is rotational velocity vector +// Wrench : e.g. [1,2,3,4,5,6] +// where [1,2,3] represents a force vector +// where [4,5,6] represents a torque vector +// Rotation : output : +// [1,2,3; +// 4,5,6; +// 7,8,9] cfr definition of Rotation object. +// input : +// 1) like the output +// 2) EulerZYX,EulerZYZ,RPY word followed by a vector, e.g. : +// Eulerzyx[10,20,30] +// (ANGLES are always expressed in DEGREES for I/O) +// (ANGELS are always expressed in RADIANS for internal representation) +// 3) Rot [1,2,3] [20] Rotates around axis [1,2,3] with an angle +// of 20 degrees. +// 4) Identity returns identity rotation matrix. +// Frames : output : [ Rotationmatrix positionvector ] +// e.g. [ [1,0,0;0,1,0;0,0,1] [1,2,3] ] +// Input : +// 1) [ Rotationmatrix positionvector ] +// 2) DH [ 10,10,50,30] Denavit-Hartenberg representation +// ( is in fact not the representation of a Frame, but more +// limited, cfr. documentation of Frame object.) +// \endverbatim +// +// \warning +// You can use iostream.h or iostream header files for file I/O, +// if one declares the define WANT_STD_IOSTREAM then the standard C++ +// iostreams headers are included instead of the compiler-dependent version +// + * + ****************************************************************************/ +#ifndef FRAMES_IO_H +#define FRAMES_IO_H + +#include "utilities/utility_io.h" +#include "frames.hpp" +#include "jntarray.hpp" +#include "jacobian.hpp" + +namespace KDL { + + //! width to be used when printing variables out with frames_io.h + //! global variable, can be changed. + + + // I/O to C++ stream. + std::ostream& operator << (std::ostream& os,const Vector& v); + std::ostream& operator << (std::ostream& os,const Rotation& R); + std::ostream& operator << (std::ostream& os,const Frame& T); + std::ostream& operator << (std::ostream& os,const Twist& T); + std::ostream& operator << (std::ostream& os,const Wrench& T); + std::ostream& operator << (std::ostream& os,const Vector2& v); + std::ostream& operator << (std::ostream& os,const Rotation2& R); + std::ostream& operator << (std::ostream& os,const Frame2& T); + + + + std::istream& operator >> (std::istream& is,Vector& v); + std::istream& operator >> (std::istream& is,Rotation& R); + std::istream& operator >> (std::istream& is,Frame& T); + std::istream& operator >> (std::istream& os,Twist& T); + std::istream& operator >> (std::istream& os,Wrench& T); + std::istream& operator >> (std::istream& is,Vector2& v); + std::istream& operator >> (std::istream& is,Rotation2& R); + std::istream& operator >> (std::istream& is,Frame2& T); + + +} // namespace Frame + +#endif diff --git a/intern/itasc/kdl/framevel.cpp b/intern/itasc/kdl/framevel.cpp new file mode 100644 index 00000000000..f70bef2e923 --- /dev/null +++ b/intern/itasc/kdl/framevel.cpp @@ -0,0 +1,27 @@ +/***************************************************************************** + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + ****************************************************************************/ + + +#include "framevel.hpp" + +namespace KDL { + +#ifndef KDL_INLINE + #include "framevel.inl" +#endif + + + +} diff --git a/intern/itasc/kdl/framevel.hpp b/intern/itasc/kdl/framevel.hpp new file mode 100644 index 00000000000..21a7844f522 --- /dev/null +++ b/intern/itasc/kdl/framevel.hpp @@ -0,0 +1,382 @@ +/***************************************************************************** + * \file + * This file contains the definition of classes for a + * Rall Algebra of (subset of) the classes defined in frames, + * i.e. classes that contain a pair (value,derivative) and define operations on that pair + * this classes are usefull for automatic differentiation ( <-> symbolic diff , <-> numeric diff) + * Defines VectorVel, RotationVel, FrameVel. Look at Frames.h for details on how to work + * with Frame objects. + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id: framevel.hpp 19905 2009-04-23 13:29:54Z ben2610 $ + * $Name: $ + ****************************************************************************/ + +#ifndef KDL_FRAMEVEL_H +#define KDL_FRAMEVEL_H + +#include "utilities/utility.h" +#include "utilities/rall1d.h" +#include "utilities/traits.h" + +#include "frames.hpp" + + + +namespace KDL { + +typedef Rall1d<double> doubleVel; + +IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) { + return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt); +} + +IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) { + return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt); +} + +IMETHOD void random(doubleVel& F) { + random(F.t); + random(F.grad); +} +IMETHOD void posrandom(doubleVel& F) { + posrandom(F.t); + posrandom(F.grad); +} + +} + +template <> +struct Traits<KDL::doubleVel> { + typedef double valueType; + typedef KDL::doubleVel derivType; +}; + +namespace KDL { + +class TwistVel; +class VectorVel; +class FrameVel; +class RotationVel; + +class VectorVel +// = TITLE +// An VectorVel is a Vector and its first derivative +// = CLASS TYPE +// Concrete +{ +public: + Vector p; // position vector + Vector v; // velocity vector +public: + VectorVel():p(),v(){} + VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {} + explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {} + + Vector value() const { return p;} + Vector deriv() const { return v;} + + IMETHOD VectorVel& operator = (const VectorVel& arg); + IMETHOD VectorVel& operator = (const Vector& arg); + IMETHOD VectorVel& operator += (const VectorVel& arg); + IMETHOD VectorVel& operator -= (const VectorVel& arg); + IMETHOD static VectorVel Zero(); + IMETHOD void ReverseSign(); + IMETHOD doubleVel Norm() const; + IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2); + IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2); + IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2); + IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2); + IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2); + IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2); + IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1); + IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x); + + IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2); + IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1); + IMETHOD friend void SetToZero(VectorVel& v); + + + IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon); + IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon); + IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon); + IMETHOD friend VectorVel operator - (const VectorVel& r); + IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs); + IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs); + IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs); +}; + + + +class RotationVel +// = TITLE +// An RotationVel is a Rotation and its first derivative, a rotation vector +// = CLASS TYPE +// Concrete +{ +public: + Rotation R; // Rotation matrix + Vector w; // rotation vector +public: + RotationVel():R(),w() {} + explicit RotationVel(const Rotation& _R):R(_R),w(Vector::Zero()){} + RotationVel(const Rotation& _R,const Vector& _w):R(_R),w(_w){} + + + Rotation value() const { return R;} + Vector deriv() const { return w;} + + + IMETHOD RotationVel& operator = (const RotationVel& arg); + IMETHOD RotationVel& operator = (const Rotation& arg); + IMETHOD VectorVel UnitX() const; + IMETHOD VectorVel UnitY() const; + IMETHOD VectorVel UnitZ() const; + IMETHOD static RotationVel Identity(); + IMETHOD RotationVel Inverse() const; + IMETHOD VectorVel Inverse(const VectorVel& arg) const; + IMETHOD VectorVel Inverse(const Vector& arg) const; + IMETHOD VectorVel operator*(const VectorVel& arg) const; + IMETHOD VectorVel operator*(const Vector& arg) const; + IMETHOD void DoRotX(const doubleVel& angle); + IMETHOD void DoRotY(const doubleVel& angle); + IMETHOD void DoRotZ(const doubleVel& angle); + IMETHOD static RotationVel RotX(const doubleVel& angle); + IMETHOD static RotationVel RotY(const doubleVel& angle); + IMETHOD static RotationVel RotZ(const doubleVel& angle); + IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle); + // rotvec has arbitrary norm + // rotation around a constant vector ! + IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle); + // rotvec is normalized. + // rotation around a constant vector ! + IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2); + IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2); + IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2); + IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon); + IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon); + IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon); + + IMETHOD TwistVel Inverse(const TwistVel& arg) const; + IMETHOD TwistVel Inverse(const Twist& arg) const; + IMETHOD TwistVel operator * (const TwistVel& arg) const; + IMETHOD TwistVel operator * (const Twist& arg) const; +}; + + + + +class FrameVel +// = TITLE +// An FrameVel is a Frame and its first derivative, a Twist vector +// = CLASS TYPE +// Concrete +// = CAVEATS +// +{ +public: + RotationVel M; + VectorVel p; +public: + FrameVel(){} + + explicit FrameVel(const Frame& _T): + M(_T.M),p(_T.p) {} + + FrameVel(const Frame& _T,const Twist& _t): + M(_T.M,_t.rot),p(_T.p,_t.vel) {} + + FrameVel(const RotationVel& _M,const VectorVel& _p): + M(_M),p(_p) {} + + + Frame value() const { return Frame(M.value(),p.value());} + Twist deriv() const { return Twist(p.deriv(),M.deriv());} + + + IMETHOD FrameVel& operator = (const Frame& arg); + IMETHOD FrameVel& operator = (const FrameVel& arg); + IMETHOD static FrameVel Identity(); + IMETHOD FrameVel Inverse() const; + IMETHOD VectorVel Inverse(const VectorVel& arg) const; + IMETHOD VectorVel operator*(const VectorVel& arg) const; + IMETHOD VectorVel operator*(const Vector& arg) const; + IMETHOD VectorVel Inverse(const Vector& arg) const; + IMETHOD Frame GetFrame() const; + IMETHOD Twist GetTwist() const; + IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2); + IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2); + IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2); + IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon); + IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon); + IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon); + + IMETHOD TwistVel Inverse(const TwistVel& arg) const; + IMETHOD TwistVel Inverse(const Twist& arg) const; + IMETHOD TwistVel operator * (const TwistVel& arg) const; + IMETHOD TwistVel operator * (const Twist& arg) const; +}; + + + + + +//very similar to Wrench class. +class TwistVel +// = TITLE +// This class represents a TwistVel. This is a velocity and rotational velocity together +{ +public: + VectorVel vel; + VectorVel rot; +public: + +// = Constructors + TwistVel():vel(),rot() {}; + TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {}; + TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {}; + TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {}; + + Twist value() const { + return Twist(vel.value(),rot.value()); + } + Twist deriv() const { + return Twist(vel.deriv(),rot.deriv()); + } +// = Operators + IMETHOD TwistVel& operator-=(const TwistVel& arg); + IMETHOD TwistVel& operator+=(const TwistVel& arg); + +// = External operators + IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs); + IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs); + IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs); + + IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs); + IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs); + IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs); + + IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs); + IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs); + IMETHOD friend TwistVel operator-(const TwistVel& arg); + IMETHOD friend void SetToZero(TwistVel& v); + + +// = Zero + static IMETHOD TwistVel Zero(); + +// = Reverse Sign + IMETHOD void ReverseSign(); + +// = Change Reference point + IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB); + // Changes the reference point of the TwistVel. + // The VectorVel v_base_AB is expressed in the same base as the TwistVel + // The VectorVel v_base_AB is a VectorVel from the old point to + // the new point. + // Complexity : 6M+6A + + // = Equality operators + // do not use operator == because the definition of Equal(.,.) is slightly + // different. It compares whether the 2 arguments are equal in an eps-interval + IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon); + IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon); + IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon); + +// = Conversion to other entities + IMETHOD Twist GetTwist() const; + IMETHOD Twist GetTwistDot() const; +// = Friends + friend class RotationVel; + friend class FrameVel; + +}; + +IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) { + return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt)); +} + +IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) { + return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt)); +} +IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) { + return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt)); +} + +IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) { + return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt)); +} + +IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) { + return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt)); +} + +IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) { + return FrameVel( + addDelta(a.M,da.rot,dt), + addDelta(a.p,da.vel,dt) + ); +} + +IMETHOD void random(VectorVel& a) { + random(a.p); + random(a.v); +} +IMETHOD void random(TwistVel& a) { + random(a.vel); + random(a.rot); +} + +IMETHOD void random(RotationVel& R) { + random(R.R); + random(R.w); +} + +IMETHOD void random(FrameVel& F) { + random(F.M); + random(F.p); +} +IMETHOD void posrandom(VectorVel& a) { + posrandom(a.p); + posrandom(a.v); +} +IMETHOD void posrandom(TwistVel& a) { + posrandom(a.vel); + posrandom(a.rot); +} + +IMETHOD void posrandom(RotationVel& R) { + posrandom(R.R); + posrandom(R.w); +} + +IMETHOD void posrandom(FrameVel& F) { + posrandom(F.M); + posrandom(F.p); +} + +#ifdef KDL_INLINE +#include "framevel.inl" +#endif + +} // namespace + +#endif + + + + diff --git a/intern/itasc/kdl/framevel.inl b/intern/itasc/kdl/framevel.inl new file mode 100644 index 00000000000..994b3d2028e --- /dev/null +++ b/intern/itasc/kdl/framevel.inl @@ -0,0 +1,534 @@ +/***************************************************************************** + * \file + * provides inline functions of rframes.h + * + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id: framevel.inl 19905 2009-04-23 13:29:54Z ben2610 $ + * $Name: $ + ****************************************************************************/ + + +// Methods and operators related to FrameVelVel +// They all delegate most of the work to RotationVelVel and VectorVelVel +FrameVel& FrameVel::operator = (const FrameVel& arg) { + M=arg.M; + p=arg.p; + return *this; +} + +FrameVel FrameVel::Identity() { + return FrameVel(RotationVel::Identity(),VectorVel::Zero()); +} + + +FrameVel operator *(const FrameVel& lhs,const FrameVel& rhs) +{ + return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} +FrameVel operator *(const FrameVel& lhs,const Frame& rhs) +{ + return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} +FrameVel operator *(const Frame& lhs,const FrameVel& rhs) +{ + return FrameVel(lhs.M*rhs.M , lhs.M*rhs.p+lhs.p ); +} + +VectorVel FrameVel::operator *(const VectorVel & arg) const +{ + return M*arg+p; +} +VectorVel FrameVel::operator *(const Vector & arg) const +{ + return M*arg+p; +} + +VectorVel FrameVel::Inverse(const VectorVel& arg) const +{ + return M.Inverse(arg-p); +} + +VectorVel FrameVel::Inverse(const Vector& arg) const +{ + return M.Inverse(arg-p); +} + +FrameVel FrameVel::Inverse() const +{ + return FrameVel(M.Inverse(),-M.Inverse(p)); +} + +FrameVel& FrameVel::operator = (const Frame& arg) { + M = arg.M; + p = arg.p; + return *this; +} +bool Equal(const FrameVel& r1,const FrameVel& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} +bool Equal(const Frame& r1,const FrameVel& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} +bool Equal(const FrameVel& r1,const Frame& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} + +Frame FrameVel::GetFrame() const { + return Frame(M.R,p.p); +} + +Twist FrameVel::GetTwist() const { + return Twist(p.v,M.w); +} + + +RotationVel operator* (const RotationVel& r1,const RotationVel& r2) { + return RotationVel( r1.R*r2.R, r1.w + r1.R*r2.w ); +} + +RotationVel operator* (const Rotation& r1,const RotationVel& r2) { + return RotationVel( r1*r2.R, r1*r2.w ); +} + +RotationVel operator* (const RotationVel& r1,const Rotation& r2) { + return RotationVel( r1.R*r2, r1.w ); +} + +RotationVel& RotationVel::operator = (const RotationVel& arg) { + R=arg.R; + w=arg.w; + return *this; + } +RotationVel& RotationVel::operator = (const Rotation& arg) { + R=arg; + w=Vector::Zero(); + return *this; +} + +VectorVel RotationVel::UnitX() const { + return VectorVel(R.UnitX(),w*R.UnitX()); +} + +VectorVel RotationVel::UnitY() const { + return VectorVel(R.UnitY(),w*R.UnitY()); +} + +VectorVel RotationVel::UnitZ() const { + return VectorVel(R.UnitZ(),w*R.UnitZ()); +} + + + +RotationVel RotationVel::Identity() { + return RotationVel(Rotation::Identity(),Vector::Zero()); +} + +RotationVel RotationVel::Inverse() const { + return RotationVel(R.Inverse(),-R.Inverse(w)); +} + +VectorVel RotationVel::Inverse(const VectorVel& arg) const { + Vector tmp=R.Inverse(arg.p); + return VectorVel(tmp, + R.Inverse(arg.v-w*arg.p) + ); +} + +VectorVel RotationVel::Inverse(const Vector& arg) const { + Vector tmp=R.Inverse(arg); + return VectorVel(tmp, + R.Inverse(-w*arg) + ); +} + + +VectorVel RotationVel::operator*(const VectorVel& arg) const { + Vector tmp=R*arg.p; + return VectorVel(tmp,w*tmp+R*arg.v); +} + +VectorVel RotationVel::operator*(const Vector& arg) const { + Vector tmp=R*arg; + return VectorVel(tmp,w*tmp); +} + + +// = Rotations +// The Rot... static functions give the value of the appropriate rotation matrix back. +// The DoRot... functions apply a rotation R to *this,such that *this = *this * R. + +void RotationVel::DoRotX(const doubleVel& angle) { + w+=R*Vector(angle.grad,0,0); + R.DoRotX(angle.t); +} +RotationVel RotationVel::RotX(const doubleVel& angle) { + return RotationVel(Rotation::RotX(angle.t),Vector(angle.grad,0,0)); +} + +void RotationVel::DoRotY(const doubleVel& angle) { + w+=R*Vector(0,angle.grad,0); + R.DoRotY(angle.t); +} +RotationVel RotationVel::RotY(const doubleVel& angle) { + return RotationVel(Rotation::RotX(angle.t),Vector(0,angle.grad,0)); +} + +void RotationVel::DoRotZ(const doubleVel& angle) { + w+=R*Vector(0,0,angle.grad); + R.DoRotZ(angle.t); +} +RotationVel RotationVel::RotZ(const doubleVel& angle) { + return RotationVel(Rotation::RotZ(angle.t),Vector(0,0,angle.grad)); +} + + +RotationVel RotationVel::Rot(const Vector& rotvec,const doubleVel& angle) +// rotvec has arbitrary norm +// rotation around a constant vector ! +{ + Vector v(rotvec); + v.Normalize(); + return RotationVel(Rotation::Rot2(v,angle.t),v*angle.grad); +} + +RotationVel RotationVel::Rot2(const Vector& rotvec,const doubleVel& angle) + // rotvec is normalized. +{ + return RotationVel(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad); +} + + +VectorVel operator + (const VectorVel& r1,const VectorVel& r2) { + return VectorVel(r1.p+r2.p,r1.v+r2.v); +} + +VectorVel operator - (const VectorVel& r1,const VectorVel& r2) { + return VectorVel(r1.p-r2.p,r1.v-r2.v); +} + +VectorVel operator + (const VectorVel& r1,const Vector& r2) { + return VectorVel(r1.p+r2,r1.v); +} + +VectorVel operator - (const VectorVel& r1,const Vector& r2) { + return VectorVel(r1.p-r2,r1.v); +} + +VectorVel operator + (const Vector& r1,const VectorVel& r2) { + return VectorVel(r1+r2.p,r2.v); +} + +VectorVel operator - (const Vector& r1,const VectorVel& r2) { + return VectorVel(r1-r2.p,-r2.v); +} + +// unary - +VectorVel operator - (const VectorVel& r) { + return VectorVel(-r.p,-r.v); +} + +void SetToZero(VectorVel& v){ + SetToZero(v.p); + SetToZero(v.v); +} + +// cross prod. +VectorVel operator * (const VectorVel& r1,const VectorVel& r2) { + return VectorVel(r1.p*r2.p, r1.p*r2.v+r1.v*r2.p); +} + +VectorVel operator * (const VectorVel& r1,const Vector& r2) { + return VectorVel(r1.p*r2, r1.v*r2); +} + +VectorVel operator * (const Vector& r1,const VectorVel& r2) { + return VectorVel(r1*r2.p, r1*r2.v); +} + + + +// scalar mult. +VectorVel operator * (double r1,const VectorVel& r2) { + return VectorVel(r1*r2.p, r1*r2.v); +} + +VectorVel operator * (const VectorVel& r1,double r2) { + return VectorVel(r1.p*r2, r1.v*r2); +} + + + +VectorVel operator * (const doubleVel& r1,const VectorVel& r2) { + return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p); +} + +VectorVel operator * (const VectorVel& r2,const doubleVel& r1) { + return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p); +} + +VectorVel operator / (const VectorVel& r1,double r2) { + return VectorVel(r1.p/r2, r1.v/r2); +} + +VectorVel operator / (const VectorVel& r2,const doubleVel& r1) { + return VectorVel(r2.p/r1.t, r2.v/r1.t - r2.p*r1.grad/r1.t/r1.t); +} + +VectorVel operator*(const Rotation& R,const VectorVel& x) { + return VectorVel(R*x.p,R*x.v); +} + +VectorVel& VectorVel::operator = (const VectorVel& arg) { + p=arg.p; + v=arg.v; + return *this; +} +VectorVel& VectorVel::operator = (const Vector& arg) { + p=arg; + v=Vector::Zero(); + return *this; +} +VectorVel& VectorVel::operator += (const VectorVel& arg) { + p+=arg.p; + v+=arg.v; + return *this; +} +VectorVel& VectorVel::operator -= (const VectorVel& arg) { + p-=arg.p; + v-=arg.v; + return *this; +} + +VectorVel VectorVel::Zero() { + return VectorVel(Vector::Zero(),Vector::Zero()); +} +void VectorVel::ReverseSign() { + p.ReverseSign(); + v.ReverseSign(); +} +doubleVel VectorVel::Norm() const { + double n = p.Norm(); + return doubleVel(n,dot(p,v)/n); +} + +bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) { + return (Equal(r1.p,r2.p,eps) && Equal(r1.v,r2.v,eps)); +} +bool Equal(const Vector& r1,const VectorVel& r2,double eps) { + return (Equal(r1,r2.p,eps) && Equal(Vector::Zero(),r2.v,eps)); +} +bool Equal(const VectorVel& r1,const Vector& r2,double eps) { + return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps)); +} + +bool Equal(const RotationVel& r1,const RotationVel& r2,double eps) { + return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps)); +} +bool Equal(const Rotation& r1,const RotationVel& r2,double eps) { + return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps)); +} +bool Equal(const RotationVel& r1,const Rotation& r2,double eps) { + return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps)); +} +bool Equal(const TwistVel& a,const TwistVel& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} +bool Equal(const Twist& a,const TwistVel& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} +bool Equal(const TwistVel& a,const Twist& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} + + + +IMETHOD doubleVel dot(const VectorVel& lhs,const VectorVel& rhs) { + return doubleVel(dot(lhs.p,rhs.p),dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p)); +} +IMETHOD doubleVel dot(const VectorVel& lhs,const Vector& rhs) { + return doubleVel(dot(lhs.p,rhs),dot(lhs.v,rhs)); +} +IMETHOD doubleVel dot(const Vector& lhs,const VectorVel& rhs) { + return doubleVel(dot(lhs,rhs.p),dot(lhs,rhs.v)); +} + + + + + + + + + + + + +TwistVel TwistVel::Zero() +{ + return TwistVel(VectorVel::Zero(),VectorVel::Zero()); +} + + +void TwistVel::ReverseSign() +{ + vel.ReverseSign(); + rot.ReverseSign(); +} + +TwistVel TwistVel::RefPoint(const VectorVel& v_base_AB) + // Changes the reference point of the TwistVel. + // The VectorVel v_base_AB is expressed in the same base as the TwistVel + // The VectorVel v_base_AB is a VectorVel from the old point to + // the new point. + // Complexity : 6M+6A +{ + return TwistVel(this->vel+this->rot*v_base_AB,this->rot); +} + +TwistVel& TwistVel::operator-=(const TwistVel& arg) +{ + vel-=arg.vel; + rot -=arg.rot; + return *this; +} + +TwistVel& TwistVel::operator+=(const TwistVel& arg) +{ + vel+=arg.vel; + rot +=arg.rot; + return *this; +} + + +TwistVel operator*(const TwistVel& lhs,double rhs) +{ + return TwistVel(lhs.vel*rhs,lhs.rot*rhs); +} + +TwistVel operator*(double lhs,const TwistVel& rhs) +{ + return TwistVel(lhs*rhs.vel,lhs*rhs.rot); +} + +TwistVel operator/(const TwistVel& lhs,double rhs) +{ + return TwistVel(lhs.vel/rhs,lhs.rot/rhs); +} + + +TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs) +{ + return TwistVel(lhs.vel*rhs,lhs.rot*rhs); +} + +TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs) +{ + return TwistVel(lhs*rhs.vel,lhs*rhs.rot); +} + +TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs) +{ + return TwistVel(lhs.vel/rhs,lhs.rot/rhs); +} + + + +// addition of TwistVel's +TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs) +{ + return TwistVel(lhs.vel+rhs.vel,lhs.rot+rhs.rot); +} + +TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs) +{ + return TwistVel(lhs.vel-rhs.vel,lhs.rot-rhs.rot); +} + +// unary - +TwistVel operator-(const TwistVel& arg) +{ + return TwistVel(-arg.vel,-arg.rot); +} + +void SetToZero(TwistVel& v) +{ + SetToZero(v.vel); + SetToZero(v.rot); +} + + + + + +TwistVel RotationVel::Inverse(const TwistVel& arg) const +{ + return TwistVel(Inverse(arg.vel),Inverse(arg.rot)); +} + +TwistVel RotationVel::operator * (const TwistVel& arg) const +{ + return TwistVel((*this)*arg.vel,(*this)*arg.rot); +} + +TwistVel RotationVel::Inverse(const Twist& arg) const +{ + return TwistVel(Inverse(arg.vel),Inverse(arg.rot)); +} + +TwistVel RotationVel::operator * (const Twist& arg) const +{ + return TwistVel((*this)*arg.vel,(*this)*arg.rot); +} + + +TwistVel FrameVel::operator * (const TwistVel& arg) const +{ + TwistVel tmp; + tmp.rot = M*arg.rot; + tmp.vel = M*arg.vel+p*tmp.rot; + return tmp; +} + +TwistVel FrameVel::operator * (const Twist& arg) const +{ + TwistVel tmp; + tmp.rot = M*arg.rot; + tmp.vel = M*arg.vel+p*tmp.rot; + return tmp; +} + +TwistVel FrameVel::Inverse(const TwistVel& arg) const +{ + TwistVel tmp; + tmp.rot = M.Inverse(arg.rot); + tmp.vel = M.Inverse(arg.vel-p*arg.rot); + return tmp; +} + +TwistVel FrameVel::Inverse(const Twist& arg) const +{ + TwistVel tmp; + tmp.rot = M.Inverse(arg.rot); + tmp.vel = M.Inverse(arg.vel-p*arg.rot); + return tmp; +} + +Twist TwistVel::GetTwist() const { + return Twist(vel.p,rot.p); +} + +Twist TwistVel::GetTwistDot() const { + return Twist(vel.v,rot.v); +} diff --git a/intern/itasc/kdl/inertia.cpp b/intern/itasc/kdl/inertia.cpp new file mode 100644 index 00000000000..6c7337d0dc4 --- /dev/null +++ b/intern/itasc/kdl/inertia.cpp @@ -0,0 +1,48 @@ +// 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 + +#include "inertia.hpp" + +#include <Eigen/Array> + +namespace KDL { +using namespace Eigen; + +Inertia::Inertia(double m,double Ixx,double Iyy,double Izz,double Ixy,double Ixz,double Iyz): +data(Matrix<double,6,6>::Zero()) +{ + data(0,0)=Ixx; + data(1,1)=Iyy; + data(2,2)=Izz; + data(2,1)=data(1,2)=Ixy; + data(3,1)=data(1,3)=Ixz; + data(3,2)=data(2,3)=Iyz; + + data.block(3,3,3,3)=m*Matrix<double,3,3>::Identity(); +} + +Inertia::~Inertia() +{ +} + + + +} diff --git a/intern/itasc/kdl/inertia.hpp b/intern/itasc/kdl/inertia.hpp new file mode 100644 index 00000000000..9f33859671c --- /dev/null +++ b/intern/itasc/kdl/inertia.hpp @@ -0,0 +1,70 @@ +// 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 KDLINERTIA_HPP +#define KDLINERTIA_HPP + +#include <Eigen/Array> +#include "frames.hpp" + +namespace KDL { + +using namespace Eigen; + +/** + * This class offers the inertia-structure of a body + * An inertia is defined in a certain reference point and a certain reference base. + * The reference point does not have to coincide with the origin of the reference frame. + */ +class Inertia{ +public: + + /** + * This constructor creates a cartesian space inertia matrix, + * the arguments are the mass and the inertia moments in the cog. + */ + Inertia(double m=0,double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0); + + static inline Inertia Zero(){ + return Inertia(0,0,0,0,0,0,0); + }; + + friend class Rotation; + friend class Frame; + + /** + * F = m*a + */ + // Wrench operator* (const AccelerationTwist& acc); + + + ~Inertia(); +private: + Matrix<double,6,6,RowMajor> data; + +}; + + + + +} + +#endif diff --git a/intern/itasc/kdl/jacobian.cpp b/intern/itasc/kdl/jacobian.cpp new file mode 100644 index 00000000000..f8f46b32619 --- /dev/null +++ b/intern/itasc/kdl/jacobian.cpp @@ -0,0 +1,129 @@ +// 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 + +#include "jacobian.hpp" + +namespace KDL +{ + Jacobian::Jacobian(unsigned int _size,unsigned int _nr_blocks): + size(_size),nr_blocks(_nr_blocks) + { + twists = new Twist[size*nr_blocks]; + } + + Jacobian::Jacobian(const Jacobian& arg): + size(arg.columns()), + nr_blocks(arg.nr_blocks) + { + twists = new Twist[size*nr_blocks]; + for(unsigned int i=0;i<size*nr_blocks;i++) + twists[i] = arg.twists[i]; + } + + Jacobian& Jacobian::operator = (const Jacobian& arg) + { + assert(size==arg.size); + assert(nr_blocks==arg.nr_blocks); + for(unsigned int i=0;i<size;i++) + twists[i]=arg.twists[i]; + return *this; + } + + + Jacobian::~Jacobian() + { + delete [] twists; + } + + double Jacobian::operator()(int i,int j)const + { + assert(i<6*(int)nr_blocks&&j<(int)size); + return twists[j+6*(int)(floor((double)i/6))](i%6); + } + + double& Jacobian::operator()(int i,int j) + { + assert(i<6*(int)nr_blocks&&j<(int)size); + return twists[j+6*(int)(floor((double)i/6))](i%6); + } + + unsigned int Jacobian::rows()const + { + return 6*nr_blocks; + } + + unsigned int Jacobian::columns()const + { + return size; + } + + void SetToZero(Jacobian& jac) + { + for(unsigned int i=0;i<jac.size*jac.nr_blocks;i++) + SetToZero(jac.twists[i]); + } + + void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest) + { + assert(src1.size==dest.size); + assert(src1.nr_blocks==dest.nr_blocks); + for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++) + dest.twists[i]=src1.twists[i].RefPoint(base_AB); + } + + void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest) + { + assert(src1.size==dest.size); + assert(src1.nr_blocks==dest.nr_blocks); + for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++) + dest.twists[i]=rot*src1.twists[i]; + } + + void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest) + { + assert(src1.size==dest.size); + assert(src1.nr_blocks==dest.nr_blocks); + for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++) + dest.twists[i]=frame*src1.twists[i]; + } + + bool Jacobian::operator ==(const Jacobian& arg) + { + return Equal((*this),arg); + } + + bool Jacobian::operator!=(const Jacobian& arg) + { + return !Equal((*this),arg); + } + + bool Equal(const Jacobian& a,const Jacobian& b,double eps) + { + if(a.rows()==b.rows()&&a.columns()==b.columns()){ + bool rc=true; + for(unsigned int i=0;i<a.columns();i++) + rc&=Equal(a.twists[i],b.twists[i],eps); + return rc; + }else + return false; + } + +} diff --git a/intern/itasc/kdl/jacobian.hpp b/intern/itasc/kdl/jacobian.hpp new file mode 100644 index 00000000000..e9057451c9f --- /dev/null +++ b/intern/itasc/kdl/jacobian.hpp @@ -0,0 +1,68 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_JACOBIAN_HPP +#define KDL_JACOBIAN_HPP + +#include "frames.hpp" + +namespace KDL +{ + //Forward declaration + class ChainJntToJacSolver; + + class Jacobian + { + friend class ChainJntToJacSolver; + private: + unsigned int size; + unsigned int nr_blocks; + public: + Twist* twists; + Jacobian(unsigned int size,unsigned int nr=1); + Jacobian(const Jacobian& arg); + + Jacobian& operator=(const Jacobian& arg); + + bool operator ==(const Jacobian& arg); + bool operator !=(const Jacobian& arg); + + friend bool Equal(const Jacobian& a,const Jacobian& b,double eps=epsilon); + + + ~Jacobian(); + + double operator()(int i,int j)const; + double& operator()(int i,int j); + unsigned int rows()const; + unsigned int columns()const; + + friend void SetToZero(Jacobian& jac); + + friend void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest); + friend void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest); + friend void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest); + + + }; +} + +#endif diff --git a/intern/itasc/kdl/jntarray.cpp b/intern/itasc/kdl/jntarray.cpp new file mode 100644 index 00000000000..2adb76081f3 --- /dev/null +++ b/intern/itasc/kdl/jntarray.cpp @@ -0,0 +1,152 @@ +// 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 + +#include "jntarray.hpp" + +namespace KDL +{ + JntArray::JntArray(): + size(0), + data(NULL) + { + } + + JntArray::JntArray(unsigned int _size): + size(_size) + { + assert(0 < size); + data = new double[size]; + SetToZero(*this); + } + + + JntArray::JntArray(const JntArray& arg): + size(arg.size) + { + data = ((0 < size) ? new double[size] : NULL); + for(unsigned int i=0;i<size;i++) + data[i]=arg.data[i]; + } + + JntArray& JntArray::operator = (const JntArray& arg) + { + assert(size==arg.size); + for(unsigned int i=0;i<size;i++) + data[i]=arg.data[i]; + return *this; + } + + + JntArray::~JntArray() + { + delete [] data; + } + + void JntArray::resize(unsigned int newSize) + { + delete [] data; + size = newSize; + data = new double[size]; + SetToZero(*this); + } + + double JntArray::operator()(unsigned int i,unsigned int j)const + { + assert(i<size&&j==0); + assert(0 != size); // found JntArray containing no data + return data[i]; + } + + double& JntArray::operator()(unsigned int i,unsigned int j) + { + assert(i<size&&j==0); + assert(0 != size); // found JntArray containing no data + return data[i]; + } + + unsigned int JntArray::rows()const + { + return size; + } + + unsigned int JntArray::columns()const + { + return 0; + } + + void Add(const JntArray& src1,const JntArray& src2,JntArray& dest) + { + assert(src1.size==src2.size&&src1.size==dest.size); + for(unsigned int i=0;i<dest.size;i++) + dest.data[i]=src1.data[i]+src2.data[i]; + } + + void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest) + { + assert(src1.size==src2.size&&src1.size==dest.size); + for(unsigned int i=0;i<dest.size;i++) + dest.data[i]=src1.data[i]-src2.data[i]; + } + + void Multiply(const JntArray& src,const double& factor,JntArray& dest) + { + assert(src.size==dest.size); + for(unsigned int i=0;i<dest.size;i++) + dest.data[i]=factor*src.data[i]; + } + + void Divide(const JntArray& src,const double& factor,JntArray& dest) + { + assert(src.rows()==dest.size); + for(unsigned int i=0;i<dest.size;i++) + dest.data[i]=src.data[i]/factor; + } + + void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest) + { + assert(jac.columns()==src.size); + SetToZero(dest); + for(unsigned int i=0;i<6;i++) + for(unsigned int j=0;j<src.size;j++) + dest(i)+=jac(i,j)*src.data[j]; + } + + void SetToZero(JntArray& array) + { + for(unsigned int i=0;i<array.size;i++) + array.data[i]=0; + } + + bool Equal(const JntArray& src1, const JntArray& src2,double eps) + { + assert(src1.size==src2.size); + bool ret = true; + for(unsigned int i=0;i<src1.size;i++) + ret = ret && Equal(src1.data[i],src2.data[i],eps); + return ret; + } + + bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}; + //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}; + +} + + diff --git a/intern/itasc/kdl/jntarray.hpp b/intern/itasc/kdl/jntarray.hpp new file mode 100644 index 00000000000..19ffa4343e3 --- /dev/null +++ b/intern/itasc/kdl/jntarray.hpp @@ -0,0 +1,217 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_JNTARRAY_HPP +#define KDL_JNTARRAY_HPP + +#include "frames.hpp" +#include "jacobian.hpp" + +namespace KDL +{ + /** + * @brief This class represents an fixed size array containing + * joint values of a KDL::Chain. + * + * \warning An object constructed with the default constructor provides + * a valid, but inert, object. Many of the member functions will do + * the correct thing and have no affect on this object, but some + * member functions can _NOT_ deal with an inert/empty object. These + * functions will assert() and exit the program instead. The intended use + * case for the default constructor (in an RTT/OCL setting) is outlined in + * code below - the default constructor plus the resize() function allow + * use of JntArray objects whose size is set within a configureHook() call + * (typically based on a size determined from a property). + +\code +class MyTask : public RTT::TaskContext +{ + JntArray j; + MyTask() + {} // invokes j's default constructor + + bool configureHook() + { + unsigned int size = some_property.rvalue(); + j.resize(size) + ... + } + + void updateHook() + { + ** use j here + } +}; +/endcode + + */ + + class JntArray + { + private: + unsigned int size; + double* data; + public: + /** Construct with _no_ data array + * @post NULL == data + * @post 0 == rows() + * @warning use of an object constructed like this, without + * a resize() first, may result in program exit! See class + * documentation. + */ + JntArray(); + /** + * Constructor of the joint array + * + * @param size size of the array, this cannot be changed + * afterwards. + * @pre 0 < size + * @post NULL != data + * @post 0 < rows() + * @post all elements in data have 0 value + */ + JntArray(unsigned int size); + /** Copy constructor + * @note Will correctly copy an empty object + */ + JntArray(const JntArray& arg); + ~JntArray(); + /** Resize the array + * @warning This causes a dynamic allocation (and potentially + * also a dynamic deallocation). This _will_ negatively affect + * real-time performance! + * + * @post newSize == rows() + * @post NULL != data + * @post all elements in data have 0 value + */ + void resize(unsigned int newSize); + + JntArray& operator = ( const JntArray& arg); + /** + * get_item operator for the joint array, if a second value is + * given it should be zero, since a JntArray resembles a column. + * + * + * @return the joint value at position i, starting from 0 + * @pre 0 != size (ie non-default constructor or resize() called) + */ + double operator()(unsigned int i,unsigned int j=0)const; + /** + * set_item operator, again if a second value is given it + *should be zero. + * + * @return reference to the joint value at position i,starting + *from zero. + * @pre 0 != size (ie non-default constructor or resize() called) + */ + double& operator()(unsigned int i,unsigned int j=0); + /** + * Returns the number of rows (size) of the array + * + */ + unsigned int rows()const; + /** + * Returns the number of columns of the array, always 1. + */ + unsigned int columns()const; + + /** + * Function to add two joint arrays, all the arguments must + * have the same size: A + B = C. This function is + * aliasing-safe, A or B can be the same array as C. + * + * @param src1 A + * @param src2 B + * @param dest C + */ + friend void Add(const JntArray& src1,const JntArray& src2,JntArray& dest); + /** + * Function to subtract two joint arrays, all the arguments must + * have the same size: A - B = C. This function is + * aliasing-safe, A or B can be the same array as C. + * + * @param src1 A + * @param src2 B + * @param dest C + */ + friend void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest); + /** + * Function to multiply all the array values with a scalar + * factor: A*b=C. This function is aliasing-safe, A can be the + * same array as C. + * + * @param src A + * @param factor b + * @param dest C + */ + friend void Multiply(const JntArray& src,const double& factor,JntArray& dest); + /** + * Function to divide all the array values with a scalar + * factor: A/b=C. This function is aliasing-safe, A can be the + * same array as C. + * + * @param src A + * @param factor b + * @param dest C + */ + 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==Twist::Zero() if 0==src.rows() (ie src is empty) + */ + friend void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest); + /** + * Function to set all the values of the array to 0 + * + * @param array + */ + friend void SetToZero(JntArray& array); + /** + * Function to check if two arrays are the same with a + *precision of eps + * + * @param src1 + * @param src2 + * @param eps default: epsilon + * @return true if each element of src1 is within eps of the same + * element in src2, or if both src1 and src2 have no data (ie 0==rows()) + */ + friend bool Equal(const JntArray& src1,const JntArray& src2,double eps=epsilon); + + friend bool operator==(const JntArray& src1,const JntArray& src2); + //friend bool operator!=(const JntArray& src1,const JntArray& src2); + }; + + bool operator==(const JntArray& src1,const JntArray& src2); + //bool operator!=(const JntArray& src1,const JntArray& src2); + +} + +#endif diff --git a/intern/itasc/kdl/jntarrayacc.cpp b/intern/itasc/kdl/jntarrayacc.cpp new file mode 100644 index 00000000000..3c9c67d9ef9 --- /dev/null +++ b/intern/itasc/kdl/jntarrayacc.cpp @@ -0,0 +1,170 @@ +// 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 + +#include "jntarrayacc.hpp" + +namespace KDL +{ + JntArrayAcc::JntArrayAcc(unsigned int size): + q(size),qdot(size),qdotdot(size) + { + } + JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin,const JntArray& qdotdotin): + q(qin),qdot(qdotin),qdotdot(qdotdotin) + { + assert(q.rows()==qdot.rows()&&qdot.rows()==qdotdot.rows()); + } + JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin): + q(qin),qdot(qdotin),qdotdot(q.rows()) + { + assert(q.rows()==qdot.rows()); + } + JntArrayAcc::JntArrayAcc(const JntArray& qin): + q(qin),qdot(q.rows()),qdotdot(q.rows()) + { + } + + JntArray JntArrayAcc::value()const + { + return q; + } + + JntArray JntArrayAcc::deriv()const + { + return qdot; + } + JntArray JntArrayAcc::dderiv()const + { + return qdotdot; + } + + void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest) + { + Add(src1.q,src2.q,dest.q); + Add(src1.qdot,src2.qdot,dest.qdot); + Add(src1.qdotdot,src2.qdotdot,dest.qdotdot); + } + void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest) + { + Add(src1.q,src2.q,dest.q); + Add(src1.qdot,src2.qdot,dest.qdot); + dest.qdotdot=src1.qdotdot; + } + void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest) + { + Add(src1.q,src2,dest.q); + dest.qdot=src1.qdot; + dest.qdotdot=src1.qdotdot; + } + + void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest) + { + Subtract(src1.q,src2.q,dest.q); + Subtract(src1.qdot,src2.qdot,dest.qdot); + Subtract(src1.qdotdot,src2.qdotdot,dest.qdotdot); + } + void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest) + { + Subtract(src1.q,src2.q,dest.q); + Subtract(src1.qdot,src2.qdot,dest.qdot); + dest.qdotdot=src1.qdotdot; + } + void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest) + { + Subtract(src1.q,src2,dest.q); + dest.qdot=src1.qdot; + dest.qdotdot=src1.qdotdot; + } + + void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest) + { + Multiply(src.q,factor,dest.q); + Multiply(src.qdot,factor,dest.qdot); + Multiply(src.qdotdot,factor,dest.qdotdot); + } + void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest) + { + Multiply(src.qdot,factor.grad*2,dest.qdot); + Multiply(src.qdotdot,factor.t,dest.qdotdot); + Add(dest.qdot,dest.qdotdot,dest.qdotdot); + Multiply(src.q,factor.grad,dest.q); + Multiply(src.qdot,factor.t,dest.qdot); + Add(dest.qdot,dest.q,dest.qdot); + Multiply(src.q,factor.t,dest.q); + } + void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest) + { + Multiply(src.q,factor.dd,dest.q); + Multiply(src.qdot,factor.d*2,dest.qdot); + Multiply(src.qdotdot,factor.t,dest.qdotdot); + Add(dest.qdotdot,dest.qdot,dest.qdotdot); + Add(dest.qdotdot,dest.q,dest.qdotdot); + Multiply(src.q,factor.d,dest.q); + Multiply(src.qdot,factor.t,dest.qdot); + Add(dest.qdot,dest.q,dest.qdot); + Multiply(src.q,factor.t,dest.q); + } + + void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest) + { + Divide(src.q,factor,dest.q); + Divide(src.qdot,factor,dest.qdot); + Divide(src.qdotdot,factor,dest.qdotdot); + } + void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest) + { + Multiply(src.q,(2*factor.grad*factor.grad)/(factor.t*factor.t*factor.t),dest.q); + Multiply(src.qdot,(2*factor.grad)/(factor.t*factor.t),dest.qdot); + Divide(src.qdotdot,factor.t,dest.qdotdot); + Subtract(dest.qdotdot,dest.qdot,dest.qdotdot); + Add(dest.qdotdot,dest.q,dest.qdotdot); + Multiply(src.q,factor.grad/(factor.t*factor.t),dest.q); + Divide(src.qdot,factor.t,dest.qdot); + Subtract(dest.qdot,dest.q,dest.qdot); + Divide(src.q,factor.t,dest.q); + } + void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest) + { + Multiply(src.q,(2*factor.d*factor.d)/(factor.t*factor.t*factor.t)-factor.dd/(factor.t*factor.t),dest.q); + Multiply(src.qdot,(2*factor.d)/(factor.t*factor.t),dest.qdot); + Divide(src.qdotdot,factor.t,dest.qdotdot); + Subtract(dest.qdotdot,dest.qdot,dest.qdotdot); + Add(dest.qdotdot,dest.q,dest.qdotdot); + Multiply(src.q,factor.d/(factor.t*factor.t),dest.q); + Divide(src.qdot,factor.t,dest.qdot); + Subtract(dest.qdot,dest.q,dest.qdot); + Divide(src.q,factor.t,dest.q); + } + + void SetToZero(JntArrayAcc& array) + { + SetToZero(array.q); + SetToZero(array.qdot); + SetToZero(array.qdotdot); + } + + bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps) + { + return (Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps)&&Equal(src1.qdotdot,src2.qdotdot,eps)); + } +} + + diff --git a/intern/itasc/kdl/jntarrayacc.hpp b/intern/itasc/kdl/jntarrayacc.hpp new file mode 100644 index 00000000000..275aa58f21e --- /dev/null +++ b/intern/itasc/kdl/jntarrayacc.hpp @@ -0,0 +1,66 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_JNTARRAYACC_HPP +#define KDL_JNTARRAYACC_HPP + +#include "utilities/utility.h" +#include "jntarray.hpp" +#include "jntarrayvel.hpp" +#include "frameacc.hpp" + +namespace KDL +{ + class JntArrayAcc + { + public: + JntArray q; + JntArray qdot; + JntArray qdotdot; + public: + 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); + + JntArray value()const; + JntArray deriv()const; + JntArray dderiv()const; + + friend void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest); + friend void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest); + friend void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest); + friend void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest); + friend void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest); + friend void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest); + friend void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest); + friend void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest); + friend void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest); + friend void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest); + friend void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest); + friend void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest); + friend void SetToZero(JntArrayAcc& array); + friend bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps=epsilon); + + }; +} + +#endif diff --git a/intern/itasc/kdl/jntarrayvel.cpp b/intern/itasc/kdl/jntarrayvel.cpp new file mode 100644 index 00000000000..df5c7fb0fb3 --- /dev/null +++ b/intern/itasc/kdl/jntarrayvel.cpp @@ -0,0 +1,111 @@ +// 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 + + +#include "jntarrayacc.hpp" + +namespace KDL +{ + JntArrayVel::JntArrayVel(unsigned int size): + q(size),qdot(size) + { + } + JntArrayVel::JntArrayVel(const JntArray& qin, const JntArray& qdotin): + q(qin),qdot(qdotin) + { + assert(q.rows()==qdot.rows()); + } + JntArrayVel::JntArrayVel(const JntArray& qin): + q(qin),qdot(q.rows()) + { + } + + JntArray JntArrayVel::value()const + { + return q; + } + + JntArray JntArrayVel::deriv()const + { + return qdot; + } + + void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest) + { + Add(src1.q,src2.q,dest.q); + Add(src1.qdot,src2.qdot,dest.qdot); + } + void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest) + { + Add(src1.q,src2,dest.q); + dest.qdot=src1.qdot; + } + + void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest) + { + Subtract(src1.q,src2.q,dest.q); + Subtract(src1.qdot,src2.qdot,dest.qdot); + } + void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest) + { + Subtract(src1.q,src2,dest.q); + dest.qdot=src1.qdot; + } + + void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest) + { + Multiply(src.q,factor,dest.q); + Multiply(src.qdot,factor,dest.qdot); + } + void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest) + { + Multiply(src.q,factor.grad,dest.q); + Multiply(src.qdot,factor.t,dest.qdot); + Add(dest.qdot,dest.q,dest.qdot); + Multiply(src.q,factor.t,dest.q); + } + + void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest) + { + Divide(src.q,factor,dest.q); + Divide(src.qdot,factor,dest.qdot); + } + void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest) + { + Multiply(src.q,(factor.grad/factor.t/factor.t),dest.q); + Divide(src.qdot,factor.t,dest.qdot); + Subtract(dest.qdot,dest.q,dest.qdot); + Divide(src.q,factor.t,dest.q); + } + + void SetToZero(JntArrayVel& array) + { + SetToZero(array.q); + SetToZero(array.qdot); + } + + bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps) + { + return Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps); + } +} + + diff --git a/intern/itasc/kdl/jntarrayvel.hpp b/intern/itasc/kdl/jntarrayvel.hpp new file mode 100644 index 00000000000..faa82076ebb --- /dev/null +++ b/intern/itasc/kdl/jntarrayvel.hpp @@ -0,0 +1,59 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_JNTARRAYVEL_HPP +#define KDL_JNTARRAYVEL_HPP + +#include "utilities/utility.h" +#include "jntarray.hpp" +#include "framevel.hpp" + +namespace KDL +{ + + class JntArrayVel + { + public: + JntArray q; + JntArray qdot; + public: + JntArrayVel(unsigned int size); + JntArrayVel(const JntArray& q,const JntArray& qdot); + JntArrayVel(const JntArray& q); + + JntArray value()const; + JntArray deriv()const; + + friend void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); + friend void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); + friend void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); + friend void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); + friend void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest); + friend void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); + friend void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest); + friend void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); + friend void SetToZero(JntArrayVel& array); + friend bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps=epsilon); + + }; +} + +#endif diff --git a/intern/itasc/kdl/joint.cpp b/intern/itasc/kdl/joint.cpp new file mode 100644 index 00000000000..dc5f17e5bf7 --- /dev/null +++ b/intern/itasc/kdl/joint.cpp @@ -0,0 +1,153 @@ +// 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 + +#include "joint.hpp" + +namespace KDL { + + Joint::Joint(const JointType& _type, const double& _scale, const double& _offset, + const double& _inertia, const double& _damping, const double& _stiffness): + type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) + { + // for sphere and swing, offset is not used, assume no offset + } + + Joint::Joint(const Joint& in): + type(in.type),scale(in.scale),offset(in.offset), + inertia(in.inertia),damping(in.damping),stiffness(in.stiffness) + { + } + + Joint& Joint::operator=(const Joint& in) + { + type=in.type; + scale=in.scale; + offset=in.offset; + inertia=in.inertia; + damping=in.damping; + stiffness=in.stiffness; + return *this; + } + + + Joint::~Joint() + { + } + + Frame Joint::pose(const double& q)const + { + + switch(type){ + 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 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 Sphere: + // the joint angles represent a rotation vector expressed in the base frame of the joint + // (= the frame you get when there is no offset nor rotation) + return Frame(Rot(Vector((&q)[0], (&q)[1], (&q)[2]))); + break; + case Swing: + // the joint angles represent a 2D rotation vector in the XZ planee of the base frame of the joint + // (= the frame you get when there is no offset nor rotation) + return Frame(Rot(Vector((&q)[0], 0.0, (&q)[1]))); + break; + default: + return Frame::Identity(); + break; + } + } + + Twist Joint::twist(const double& qdot, int dof)const + { + switch(type){ + 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 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 Swing: + switch (dof) { + case 0: + return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0)); + case 1: + return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot)); + } + return Twist::Zero(); + case Sphere: + switch (dof) { + case 0: + return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0)); + case 1: + return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0)); + case 2: + return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot)); + } + return Twist::Zero(); + default: + return Twist::Zero(); + break; + } + } + + unsigned int Joint::getNDof() const + { + switch (type) { + case Sphere: + return 3; + case Swing: + return 2; + case None: + return 0; + default: + return 1; + } + } + +} // end of namespace KDL + diff --git a/intern/itasc/kdl/joint.hpp b/intern/itasc/kdl/joint.hpp new file mode 100644 index 00000000000..a1291509f0f --- /dev/null +++ b/intern/itasc/kdl/joint.hpp @@ -0,0 +1,138 @@ +// 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_JOINT_HPP +#define KDL_JOINT_HPP + +#include "frames.hpp" +#include <string> + +namespace KDL { + + /** + * \brief This class encapsulates a simple joint, that is with one + * parameterized degree of freedom and with scalar dynamic properties. + * + * A simple joint is described by the following properties : + * - scale: ratio between motion input and motion output + * - offset: between the "physical" and the "logical" zero position. + * - type: revolute or translational, along one of the basic frame axes + * - inertia, stiffness and damping: scalars representing the physical + * effects along/about the joint axis only. + * + * @ingroup KinematicFamily + */ + class Joint { + public: + typedef enum { RotX,RotY,RotZ,TransX,TransY,TransZ,Sphere,Swing,None} JointType; + /** + * Constructor of a joint. + * + * @param type type of the joint, default: Joint::None + * @param scale scale between joint input and actual geometric + * movement, default: 1 + * @param offset offset between joint input and actual + * geometric input, default: 0 + * @param inertia 1D inertia along the joint axis, default: 0 + * @param damping 1D damping along the joint axis, default: 0 + * @param stiffness 1D stiffness along the joint axis, + * default: 0 + */ + Joint(const JointType& type=None,const double& scale=1,const double& offset=0, + const double& inertia=0,const double& damping=0,const double& stiffness=0); + Joint(const Joint& in); + + Joint& operator=(const Joint& arg); + + /** + * Request the 6D-pose between the beginning and the end of + * the joint at joint position q + * + * @param q the 1D joint position + * + * @return the resulting 6D-pose + */ + Frame pose(const double& q)const; + /** + * Request the resulting 6D-velocity with a joint velocity qdot + * + * @param qdot the 1D joint velocity + * + * @return the resulting 6D-velocity + */ + Twist twist(const double& qdot, int dof=0)const; + + /** + * Request the type of the joint. + * + * @return const reference to the type + */ + const JointType& getType() const + { + return type; + }; + + /** + * Request the stringified type of the joint. + * + * @return const string + */ + const std::string getTypeName() const + { + switch (type) { + case RotX: + return "RotX"; + case RotY: + return "RotY"; + case RotZ: + return "RotZ"; + case TransX: + return "TransX"; + case TransY: + return "TransY"; + case TransZ: + return "TransZ"; + case Sphere: + return "Sphere"; + case Swing: + return "Swing"; + case None: + return "None"; + default: + return "None"; + } + }; + unsigned int getNDof() const; + + virtual ~Joint(); + + private: + Joint::JointType type; + double scale; + double offset; + double inertia; + double damping; + double stiffness; + }; + +} // end of namespace KDL + +#endif diff --git a/intern/itasc/kdl/kinfam_io.cpp b/intern/itasc/kdl/kinfam_io.cpp new file mode 100644 index 00000000000..900e2e101a9 --- /dev/null +++ b/intern/itasc/kdl/kinfam_io.cpp @@ -0,0 +1,101 @@ +// 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 + +#include "kinfam_io.hpp" +#include "frames_io.hpp" + +namespace KDL { +std::ostream& operator <<(std::ostream& os, const Joint& joint) { + return os << joint.getTypeName(); +} + +std::istream& operator >>(std::istream& is, Joint& joint) { + return is; +} + +std::ostream& operator <<(std::ostream& os, const Segment& segment) { + os << "[" << segment.getJoint() << ",\n" << segment.getFrameToTip() << "]"; + return os; +} + +std::istream& operator >>(std::istream& is, Segment& segment) { + return is; +} + +std::ostream& operator <<(std::ostream& os, const Chain& chain) { + os << "["; + for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) + os << chain.getSegment(i) << "\n"; + os << "]"; + return os; +} + +std::istream& operator >>(std::istream& is, Chain& chain) { + return is; +} + +std::ostream& operator <<(std::ostream& os, const Tree& tree) { + SegmentMap::const_iterator root = tree.getSegment("root"); + 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"; + } + return os << "\n"; +} + +std::istream& operator >>(std::istream& is, Tree& tree) { + return is; +} + +std::ostream& operator <<(std::ostream& os, const JntArray& array) { + os << "["; + for (unsigned int i = 0; i < array.rows(); i++) + os << std::setw(KDL_FRAME_WIDTH) << array(i); + os << "]"; + return os; +} + +std::istream& operator >>(std::istream& is, JntArray& array) { + return is; +} + +std::ostream& operator <<(std::ostream& os, const Jacobian& jac) { + os << "["; + for (unsigned int i = 0; i < jac.rows(); i++) { + for (unsigned int j = 0; j < jac.columns(); j++) + os << std::setw(KDL_FRAME_WIDTH) << jac(i, j); + os << std::endl; + } + os << "]"; + return os; +} + +std::istream& operator >>(std::istream& is, Jacobian& jac) { + return is; +} + +} + diff --git a/intern/itasc/kdl/kinfam_io.hpp b/intern/itasc/kdl/kinfam_io.hpp new file mode 100644 index 00000000000..a8dbfd1c5dc --- /dev/null +++ b/intern/itasc/kdl/kinfam_io.hpp @@ -0,0 +1,70 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_KINFAM_IO_HPP +#define KDL_KINFAM_IO_HPP + +#include <iostream> +#include <fstream> + +#include "joint.hpp" +#include "segment.hpp" +#include "chain.hpp" +#include "jntarray.hpp" +#include "jacobian.hpp" +#include "tree.hpp" + +namespace KDL { +std::ostream& operator <<(std::ostream& os, const Joint& joint); +std::istream& operator >>(std::istream& is, Joint& joint); +std::ostream& operator <<(std::ostream& os, const Segment& segment); +std::istream& operator >>(std::istream& is, Segment& segment); +std::ostream& operator <<(std::ostream& os, const Chain& chain); +std::istream& operator >>(std::istream& is, Chain& chain); + +std::ostream& operator <<(std::ostream& os, const Tree& tree); +std::istream& operator >>(std::istream& is, Tree& tree); + +std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator it); + +std::ostream& operator <<(std::ostream& os, const JntArray& array); +std::istream& operator >>(std::istream& is, JntArray& array); +std::ostream& operator <<(std::ostream& os, const Jacobian& jac); +std::istream& operator >>(std::istream& is, Jacobian& jac); + +template<typename T> +std::ostream& operator<<(std::ostream& os, const std::vector<T>& vec) { + os << "["; + for (unsigned int i = 0; i < vec.size(); i++) + os << vec[i] << " "; + os << "]"; + return os; +} +; + +template<typename T> +std::istream& operator >>(std::istream& is, std::vector<T>& vec) { + return is; +} +; +} +#endif + diff --git a/intern/itasc/kdl/segment.cpp b/intern/itasc/kdl/segment.cpp new file mode 100644 index 00000000000..02f71d5e9f1 --- /dev/null +++ b/intern/itasc/kdl/segment.cpp @@ -0,0 +1,68 @@ +// 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 + +#include "segment.hpp" + +namespace KDL { + + Segment::Segment(const Joint& _joint, const Frame& _f_tip, const Inertia& _M): + joint(_joint),M(_M), + f_tip(_f_tip) + { + } + + Segment::Segment(const Segment& in): + joint(in.joint),M(in.M), + f_tip(in.f_tip) + { + } + + Segment& Segment::operator=(const Segment& arg) + { + joint=arg.joint; + M=arg.M; + f_tip=arg.f_tip; + return *this; + } + + Segment::~Segment() + { + } + + Frame Segment::pose(const double& q)const + { + return joint.pose(q)*f_tip; + } + + Twist Segment::twist(const double& q, const double& qdot, int dof)const + { + return joint.twist(qdot, dof).RefPoint(pose(q).p); + } + + Twist Segment::twist(const Vector& p, const double& qdot, int dof)const + { + return joint.twist(qdot, dof).RefPoint(p); + } + + Twist Segment::twist(const Frame& f, const double& qdot, int dof)const + { + return (f.M*joint.twist(qdot, dof)).RefPoint(f.p); + } +}//end of namespace KDL + diff --git a/intern/itasc/kdl/segment.hpp b/intern/itasc/kdl/segment.hpp new file mode 100644 index 00000000000..7c82ab418fa --- /dev/null +++ b/intern/itasc/kdl/segment.hpp @@ -0,0 +1,149 @@ +// 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_SEGMENT_HPP +#define KDL_SEGMENT_HPP + +#include "frames.hpp" +#include "inertia.hpp" +#include "joint.hpp" +#include <vector> + +namespace KDL { + + /** + * \brief This class encapsulates a simple segment, that is a "rigid + * body" (i.e., a frame and an inertia) with a joint and with + * "handles", root and tip to connect to other segments. + * + * A simple segment is described by the following properties : + * - Joint + * - inertia: of the rigid body part of the Segment + * - Offset from the end of the joint to the tip of the segment: + * the joint is located at the root of the segment. + * + * @ingroup KinematicFamily + */ + class Segment { + friend class Chain; + private: + Joint joint; + Inertia M; + Frame f_tip; + + public: + /** + * Constructor of the segment + * + * @param joint joint of the segment, default: + * Joint(Joint::None) + * @param f_tip frame from the end of the joint to the tip of + * the segment, default: Frame::Identity() + * @param M rigid body inertia of the segment, default: Inertia::Zero() + */ + Segment(const Joint& joint=Joint(), const Frame& f_tip=Frame::Identity(),const Inertia& M = Inertia::Zero()); + Segment(const Segment& in); + Segment& operator=(const Segment& arg); + + virtual ~Segment(); + + /** + * Request the pose of the segment, given the joint position q. + * + * @param q 1D position of the joint + * + * @return pose from the root to the tip of the segment + */ + Frame pose(const double& q)const; + /** + * Request the 6D-velocity of the tip of the segment, given + * the joint position q and the joint velocity qdot. + * + * @param q ND position of the joint + * @param qdot ND velocity of the joint + * + * @return 6D-velocity of the tip of the segment, expressed + *in the base-frame of the segment(root) and with the tip of + *the segment as reference point. + */ + Twist twist(const double& q,const double& qdot, int dof=0)const; + + /** + * Request the 6D-velocity at a given point p, relative to base frame of the segment + * givven the joint velocity qdot. + * + * @param p reference point + * @param qdot ND velocity of the joint + * + * @return 6D-velocity at a given point p, expressed + * in the base-frame of the segment(root) + */ + Twist twist(const Vector& p, const double& qdot, int dof=0)const; + + /** + * Request the 6D-velocity at a given frame origin, relative to base frame of the segment + * assuming the frame rotation is the rotation of the joint. + * + * @param f joint pose frame + reference point + * @param qdot ND velocity of the joint + * + * @return 6D-velocity at frame reference point, expressed + * in the base-frame of the segment(root) + */ + Twist twist(const Frame& f, const double& qdot, int dof)const; + + /** + * Request the joint of the segment + * + * + * @return const reference to the joint of the segment + */ + const Joint& getJoint()const + { + return joint; + } + /** + * Request the inertia of the segment + * + * + * @return const reference to the inertia of the segment + */ + const Inertia& getInertia()const + { + return M; + } + + /** + * Request the pose from the joint end to the tip of the + *segment. + * + * @return const reference to the joint end - segment tip pose. + */ + const Frame& getFrameToTip()const + { + return f_tip; + } + + }; +}//end of namespace KDL + +#endif diff --git a/intern/itasc/kdl/tree.cpp b/intern/itasc/kdl/tree.cpp new file mode 100644 index 00000000000..f117e54959b --- /dev/null +++ b/intern/itasc/kdl/tree.cpp @@ -0,0 +1,117 @@ +// 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 + +#include "tree.hpp" +#include <sstream> +namespace KDL { +using namespace std; + +Tree::Tree() : + nrOfJoints(0), nrOfSegments(0) { + segments.insert(make_pair("root", TreeElement::Root())); +} + +Tree::Tree(const Tree& in) { + segments.clear(); + nrOfSegments = 0; + nrOfJoints = 0; + + segments.insert(make_pair("root", TreeElement::Root())); + this->addTree(in, "", "root"); + +} + +Tree& Tree::operator=(const Tree& in) { + segments.clear(); + nrOfSegments = 0; + nrOfJoints = 0; + + segments.insert(make_pair("root", TreeElement::Root())); + this->addTree(in, "", "root"); + return *this; +} + +bool Tree::addSegment(const Segment& segment, const std::string& segment_name, + const std::string& hook_name) { + SegmentMap::iterator parent = segments.find(hook_name); + //check if parent exists + if (parent == segments.end()) + return false; + pair<SegmentMap::iterator, bool> retval; + //insert new element + retval = segments.insert(make_pair(segment_name, TreeElement(segment, + parent, nrOfJoints))); + //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); + //increase number of segments + nrOfSegments++; + //increase number of joints + nrOfJoints += segment.getJoint().getNDof(); + return true; +} + +bool Tree::addChain(const Chain& chain, const std::string& chain_name, + const std::string& hook_name) { + string parent_name = hook_name; + for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) { + ostringstream segment_name; + segment_name << chain_name << "Segment" << i; + if (this->addSegment(chain.getSegment(i), segment_name.str(), + parent_name)) + parent_name = segment_name.str(); + else + return false; + } + return true; +} + +bool Tree::addTree(const Tree& tree, const std::string& tree_name, + const std::string& hook_name) { + return this->addTreeRecursive(tree.getSegment("root"), tree_name, hook_name); +} + +bool Tree::addTreeRecursive(SegmentMap::const_iterator root, + const std::string& tree_name, const std::string& hook_name) { + //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]; + //Try to add the child + if (this->addSegment(child->second.segment, tree_name + child->first, + hook_name)) { + //if child is added, add all the child's children + if (!(this->addTreeRecursive(child, tree_name, tree_name + + child->first))) + //if it didn't work, return false + return false; + } else + //If the child could not be added, return false + return false; + } + return true; +} + +} + diff --git a/intern/itasc/kdl/tree.hpp b/intern/itasc/kdl/tree.hpp new file mode 100644 index 00000000000..bdd3aa94572 --- /dev/null +++ b/intern/itasc/kdl/tree.hpp @@ -0,0 +1,167 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_TREE_HPP +#define KDL_TREE_HPP + +#include "segment.hpp" +#include "chain.hpp" + +#include <string> +#include <map> + +namespace KDL +{ + //Forward declaration + class TreeElement; + typedef std::map<std::string,TreeElement> SegmentMap; + + class TreeElement + { + private: + TreeElement():q_nr(0) + {}; + public: + 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() + { + return TreeElement(); + }; + }; + + /** + * \brief This class encapsulates a <strong>tree</strong> + * kinematic interconnection structure. It is build out of segments. + * + * @ingroup KinematicFamily + */ + class Tree + { + private: + SegmentMap segments; + unsigned int nrOfJoints; + unsigned int nrOfSegments; + + bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& tree_name, const std::string& hook_name); + + public: + /** + * The constructor of a tree, a new tree is always empty + */ + Tree(); + Tree(const Tree& in); + Tree& operator= (const Tree& arg); + + /** + * Adds a new segment to the end of the segment with + * hook_name as segment_name + * + * @param segment new segment to add + * @param segment_name name of the new segment + * @param hook_name name of the segment to connect this + * segment with. + * + * @return false if hook_name could not be found. + */ + bool addSegment(const Segment& segment, const std::string& segment_name, const std::string& hook_name); + + /** + * Adds a complete chain to the end of the segment with + * hook_name as segment_name. Segment i of + * the chain will get chain_name+".Segment"+i as segment_name. + * + * @param chain Chain to add + * @param chain_name name of the chain + * @param hook_name name of the segment to connect the chain with. + * + * @return false if hook_name could not be found. + */ + bool addChain(const Chain& chain, const std::string& chain_name, const std::string& hook_name); + + /** + * Adds a complete tree to the end of the segment with + * hookname as segment_name. The segments of the tree will get + * tree_name+segment_name as segment_name. + * + * @param tree Tree to add + * @param tree_name name of the tree + * @param hook_name name of the segment to connect the tree with + * + * @return false if hook_name could not be found + */ + bool addTree(const Tree& tree, const std::string& tree_name,const std::string& hook_name); + + /** + * Request the total number of joints in the tree.\n + * <strong> Important:</strong> It is not the same as the + * total number of segments since a segment does not need to have + * a joint. + * + * @return total nr of joints + */ + unsigned int getNrOfJoints()const + { + return nrOfJoints; + }; + + /** + * Request the total number of segments in the tree. + * @return total number of segments + */ + unsigned int getNrOfSegments()const {return nrOfSegments;}; + + /** + * Request the segment of the tree with name segment_name. + * + * @param segment_name the name of the requested segment + * + * @return constant iterator pointing to the requested segment + */ + SegmentMap::const_iterator getSegment(const std::string& segment_name)const + { + return segments.find(segment_name); + }; + + + + const SegmentMap& getSegments()const + { + return segments; + } + + virtual ~Tree(){}; + }; +} +#endif + + + + + diff --git a/intern/itasc/kdl/treefksolver.hpp b/intern/itasc/kdl/treefksolver.hpp new file mode 100644 index 00000000000..22d5400ab0a --- /dev/null +++ b/intern/itasc/kdl/treefksolver.hpp @@ -0,0 +1,110 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Copyright (C) 2008 Julia Jesse + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_TREE_FKSOLVER_HPP +#define KDL_TREE_FKSOLVER_HPP + +#include <string> + +#include "tree.hpp" +//#include "framevel.hpp" +//#include "frameacc.hpp" +#include "jntarray.hpp" +//#include "jntarrayvel.hpp" +//#include "jntarrayacc.hpp" + +namespace KDL { + + /** + * \brief This <strong>abstract</strong> class encapsulates a + * solver for the forward position kinematics for a KDL::Tree. + * + * @ingroup KinematicFamily + */ + + //Forward definition + class TreeFkSolverPos { + public: + /** + * Calculate forward position kinematics for a KDL::Tree, + * from joint coordinates to cartesian pose. + * + * @param q_in input joint coordinates + * @param p_out reference to output cartesian pose + * + * @return if < 0 something went wrong + */ + virtual int JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)=0; + virtual ~TreeFkSolverPos(){}; + }; + + /** + * \brief This <strong>abstract</strong> class encapsulates a solver + * for the forward velocity kinematics for a KDL::Tree. + * + * @ingroup KinematicFamily + */ +// class TreeFkSolverVel { +// public: + /** + * Calculate forward position and velocity kinematics, from + * joint coordinates to cartesian coordinates. + * + * @param q_in input joint coordinates (position and velocity) + * @param out output cartesian coordinates (position and velocity) + * + * @return if < 0 something went wrong + */ +// virtual int JntToCart(const JntArrayVel& q_in, FrameVel& out,int segmentNr=-1)=0; + +// virtual ~TreeFkSolverVel(){}; +// }; + + /** + * \brief This <strong>abstract</strong> class encapsulates a solver + * for the forward acceleration kinematics for a KDL::Tree. + * + * @ingroup KinematicFamily + */ + +// class TreeFkSolverAcc { +// public: + /** + * Calculate forward position, velocity and accelaration + * kinematics, from joint coordinates to cartesian coordinates + * + * @param q_in input joint coordinates (position, velocity and + * acceleration + @param out output cartesian coordinates (position, velocity + * and acceleration + * + * @return if < 0 something went wrong + */ +// virtual int JntToCart(const JntArrayAcc& q_in, FrameAcc& out,int segmentNr=-1)=0; + +// virtual ~TreeFkSolverAcc()=0; +// }; + + +}//end of namespace KDL + +#endif diff --git a/intern/itasc/kdl/treefksolverpos_recursive.cpp b/intern/itasc/kdl/treefksolverpos_recursive.cpp new file mode 100644 index 00000000000..f9dcb336d5d --- /dev/null +++ b/intern/itasc/kdl/treefksolverpos_recursive.cpp @@ -0,0 +1,70 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Copyright (C) 2008 Julia Jesse + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "treefksolverpos_recursive.hpp" +#include <iostream> + +namespace KDL { + + TreeFkSolverPos_recursive::TreeFkSolverPos_recursive(const Tree& _tree): + tree(_tree) + { + } + + int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName) + { + SegmentMap::const_iterator it = tree.getSegment(segmentName); + SegmentMap::const_iterator baseit = tree.getSegment(baseName); + + if(q_in.rows() != tree.getNrOfJoints()) + return -1; + else if(it == tree.getSegments().end()) //if the segment name is not found + return -2; + else if(baseit == tree.getSegments().end()) //if the base segment name is not found + return -3; + else{ + p_out = recursiveFk(q_in, it, baseit); + return 0; + } + } + + Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit) + { + //gets the frame for the current element (segment) + const TreeElement& currentElement = it->second; + + if(it == baseit){ + return KDL::Frame::Identity(); + } + else{ + Frame currentFrame = currentElement.segment.pose(((JntArray&)q_in)(currentElement.q_nr)); + SegmentMap::const_iterator parentIt = currentElement.parent; + return recursiveFk(q_in, parentIt, baseit) * currentFrame; + } + } + + TreeFkSolverPos_recursive::~TreeFkSolverPos_recursive() + { + } + + +} diff --git a/intern/itasc/kdl/treefksolverpos_recursive.hpp b/intern/itasc/kdl/treefksolverpos_recursive.hpp new file mode 100644 index 00000000000..c22fe4af75b --- /dev/null +++ b/intern/itasc/kdl/treefksolverpos_recursive.hpp @@ -0,0 +1,53 @@ +// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Copyright (C) 2008 Julia Jesse + +// Version: 1.0 +// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDLTREEFKSOLVERPOS_RECURSIVE_HPP +#define KDLTREEFKSOLVERPOS_RECURSIVE_HPP + +#include "treefksolver.hpp" + +namespace KDL { + + /** + * Implementation of a recursive forward position kinematics + * algorithm to calculate the position transformation from joint + * space to Cartesian space of a general kinematic tree (KDL::Tree). + * + * @ingroup KinematicFamily + */ + class TreeFkSolverPos_recursive : public TreeFkSolverPos + { + public: + TreeFkSolverPos_recursive(const Tree& tree); + ~TreeFkSolverPos_recursive(); + + virtual int JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName); + + private: + const Tree tree; + + Frame recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit); + }; + +} + +#endif diff --git a/intern/itasc/kdl/treejnttojacsolver.cpp b/intern/itasc/kdl/treejnttojacsolver.cpp new file mode 100644 index 00000000000..194f18eb959 --- /dev/null +++ b/intern/itasc/kdl/treejnttojacsolver.cpp @@ -0,0 +1,78 @@ +/* + * TreeJntToJacSolver.cpp + * + * Created on: Nov 27, 2008 + * Author: rubensmits + */ + +#include "treejnttojacsolver.hpp" +#include <iostream> + +namespace KDL { + +TreeJntToJacSolver::TreeJntToJacSolver(const Tree& tree_in) : + tree(tree_in) { +} + +TreeJntToJacSolver::~TreeJntToJacSolver() { +} + +int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, + const std::string& segmentname) { + //First we check all the sizes: + if (q_in.rows() != tree.getNrOfJoints() || jac.columns() + != tree.getNrOfJoints()) + return -1; + + //Lets search the tree-element + SegmentMap::const_iterator it = tree.getSegments().find(segmentname); + + //If segmentname is not inside the tree, back out: + if (it == tree.getSegments().end()) + return -2; + + //Let's make the jacobian zero: + SetToZero(jac); + + SegmentMap::const_iterator root = tree.getSegments().find("root"); + + Frame T_total = Frame::Identity(); + Frame T_local, T_joint; + Twist t_local; + //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; + + //get the pose of the joint. + T_joint = it->second.segment.getJoint().pose(((JntArray&)q_in)(q_nr)); + // combine with the tip to have the tip pose + T_local = T_joint*it->second.segment.getFrameToTip(); + //calculate new T_end: + T_total = T_local * T_total; + + //get the twist of the segment: + int ndof = it->second.segment.getJoint().getNDof(); + for (int dof=0; dof<ndof; dof++) { + // combine joint rotation with tip position to get a reference frame for the joint + T_joint.p = T_local.p; + // in which the twist can be computed (needed for NDof joint) + t_local = it->second.segment.twist(T_joint, 1.0, dof); + //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 + t_local = T_total.M.Inverse(t_local); + //store the twist in the jacobian: + jac.twists[q_nr+dof] = t_local; + } + //goto the parent + it = it->second.parent; + }//endwhile + //Change the base of the complete jacobian from the endpoint to the base + changeBase(jac, T_total.M, jac); + + return 0; + +}//end JntToJac +}//end namespace + diff --git a/intern/itasc/kdl/treejnttojacsolver.hpp b/intern/itasc/kdl/treejnttojacsolver.hpp new file mode 100644 index 00000000000..40977dcd577 --- /dev/null +++ b/intern/itasc/kdl/treejnttojacsolver.hpp @@ -0,0 +1,38 @@ +/* + * TreeJntToJacSolver.hpp + * + * Created on: Nov 27, 2008 + * Author: rubensmits + */ + +#ifndef TREEJNTTOJACSOLVER_HPP_ +#define TREEJNTTOJACSOLVER_HPP_ + +#include "tree.hpp" +#include "jacobian.hpp" +#include "jntarray.hpp" + +namespace KDL { + +class TreeJntToJacSolver { +public: + TreeJntToJacSolver(const Tree& tree); + + virtual ~TreeJntToJacSolver(); + + /* + * Calculate the jacobian for a part of the tree: from a certain segment, given by segmentname to the root. + * The resulting jacobian is expressed in the baseframe of the tree ("root"), the reference point is in the end-segment + */ + + int JntToJac(const JntArray& q_in, Jacobian& jac, + const std::string& segmentname); + +private: + KDL::Tree tree; + +}; + +}//End of namespace + +#endif /* TREEJNTTOJACSOLVER_H_ */ diff --git a/intern/itasc/kdl/utilities/Makefile b/intern/itasc/kdl/utilities/Makefile new file mode 100644 index 00000000000..8ee08089e10 --- /dev/null +++ b/intern/itasc/kdl/utilities/Makefile @@ -0,0 +1,40 @@ +# +# $Id$ +# +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software Foundation, +# Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +# +# The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. +# All rights reserved. +# +# The Original Code is: all of this file. +# +# Contributor(s): Hans Lambermont +# +# ***** END GPL LICENSE BLOCK ***** +# iksolver main makefile. +# + +include nan_definitions.mk + +LIBNAME = itasc_kdl_util +# Same dir than parent (itasc instead of $(LIBNAME)) +DIR = $(OCGDIR)/intern/itasc + +include nan_compile.mk + +CPPFLAGS += -I. +CPPFLAGS += -I../../../../extern/Eigen2 diff --git a/intern/itasc/kdl/utilities/error.h b/intern/itasc/kdl/utilities/error.h new file mode 100644 index 00000000000..868daef3db3 --- /dev/null +++ b/intern/itasc/kdl/utilities/error.h @@ -0,0 +1,245 @@ +/*************************************************************************** + 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$ + * $Name: $ + ****************************************************************************/ +#ifndef ERROR_H_84822 // to make it unique, a random number +#define ERROR_H_84822 + +#include "utility.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 garanteed 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 occured 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 { +public: + virtual const char* Description() const { return "Motion Profile with requested parameters is not feasible";} + virtual int GetType() const {return 3004;} +}; + +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 diff --git a/intern/itasc/kdl/utilities/error_stack.cpp b/intern/itasc/kdl/utilities/error_stack.cpp new file mode 100644 index 00000000000..d55308c7346 --- /dev/null +++ b/intern/itasc/kdl/utilities/error_stack.cpp @@ -0,0 +1,59 @@ +/***************************************************************************** + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + ****************************************************************************/ + + +#include "error_stack.h" +#include <stack> +#include <vector> +#include <string> +#include <cstring> + +namespace KDL { + +// Trace of the call stack of the I/O routines to help user +// 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... + + +void IOTrace(const std::string& description) { + errorstack.push(description); +} + + +void IOTracePop() { + errorstack.pop(); +} + +void IOTraceOutput(std::ostream& os) { + while (!errorstack.empty()) { + os << errorstack.top().c_str() << std::endl; + errorstack.pop(); + } +} + + +void IOTracePopStr(char* buffer,int size) { + if (errorstack.empty()) { + *buffer = 0; + return; + } + strncpy(buffer,errorstack.top().c_str(),size); + errorstack.pop(); +} + +} diff --git a/intern/itasc/kdl/utilities/error_stack.h b/intern/itasc/kdl/utilities/error_stack.h new file mode 100644 index 00000000000..918bc0786a6 --- /dev/null +++ b/intern/itasc/kdl/utilities/error_stack.h @@ -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 accomodate 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 + diff --git a/intern/itasc/kdl/utilities/kdl-config.h b/intern/itasc/kdl/utilities/kdl-config.h new file mode 100644 index 00000000000..4d2df2df6c5 --- /dev/null +++ b/intern/itasc/kdl/utilities/kdl-config.h @@ -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 diff --git a/intern/itasc/kdl/utilities/rall1d.h b/intern/itasc/kdl/utilities/rall1d.h new file mode 100644 index 00000000000..98bd4385d1e --- /dev/null +++ b/intern/itasc/kdl/utilities/rall1d.h @@ -0,0 +1,478 @@ + +/***************************************************************************** + * \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$ + * $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 usefull when you recurse a Rall1d object into itself to create a 2nd, 3th, 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 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)); +} + +} + + + +#endif diff --git a/intern/itasc/kdl/utilities/rall2d.h b/intern/itasc/kdl/utilities/rall2d.h new file mode 100644 index 00000000000..cbd9e70b04f --- /dev/null +++ b/intern/itasc/kdl/utilities/rall2d.h @@ -0,0 +1,538 @@ + +/***************************************************************************** + * \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$ + * $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 usefull when you recurse a Rall1d object into itself to create a 2nd, 3th, 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() {} + + 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) + ); +} + + +} + + +#endif diff --git a/intern/itasc/kdl/utilities/svd_eigen_HH.hpp b/intern/itasc/kdl/utilities/svd_eigen_HH.hpp new file mode 100644 index 00000000000..2bbb8df521f --- /dev/null +++ b/intern/itasc/kdl/utilities/svd_eigen_HH.hpp @@ -0,0 +1,309 @@ +// 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/Array> +#include <algorithm> + +namespace KDL +{ + template<typename Scalar> inline Scalar PYTHAG(Scalar a,Scalar b) { + double at,bt,ct; + at = fabs(a); + bt = fabs(b); + if (at > bt ) { + ct=bt/at; + return Scalar(at*sqrt(1.0+ct*ct)); + } else { + if (bt==0) + return Scalar(0.0); + else { + ct=at/bt; + return Scalar(bt*sqrt(1.0+ct*ct)); + } + } + } + + + template<typename Scalar> inline Scalar SIGN(Scalar a,Scalar b) { + return ((b) >= Scalar(0.0) ? fabs(a) : -fabs(a)); + } + + /** + * svd calculation of boost ublas 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 + */ + template<typename MatrixA, typename MatrixUV, typename VectorS> + int svd_eigen_HH( + const Eigen::MatrixBase<MatrixA>& A, + Eigen::MatrixBase<MatrixUV>& U, + Eigen::MatrixBase<VectorS>& S, + Eigen::MatrixBase<MatrixUV>& V, + Eigen::MatrixBase<VectorS>& tmp, + int maxiter=150) + { + //get the rows/columns of the matrix + const int rows = A.rows(); + const int cols = A.cols(); + + U = A; + + int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0; + int ppi(0); + bool flag; + e_scalar maxarg1,maxarg2,anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0); + + g=scale=anorm=e_scalar(0.0); + + /* Householder reduction to bidiagonal form. */ + for (i=0;i<cols;i++) { + ppi=i+1; + tmp(i)=scale*g; + g=s=scale=e_scalar(0.0); + if (i<rows) { + // compute the sum of the i-th column, starting from the i-th row + for (k=i;k<rows;k++) scale += fabs(U(k,i)); + if (scale!=0) { + // multiply the i-th column by 1.0/scale, start from the i-th element + // sum of squares of column i, start from the i-th element + for (k=i;k<rows;k++) { + U(k,i) /= scale; + s += U(k,i)*U(k,i); + } + f=U(i,i); // f is the diag elem + g = -SIGN(e_scalar(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); + 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); + } + for (k=i;k<rows;k++) U(k,i) *= scale; + } + } + // save singular value + S(i)=scale*g; + g=s=scale=e_scalar(0.0); + if ((i <rows) && (i+1 != cols)) { + // sum of row i, start from columns i+1 + for (k=ppi;k<cols;k++) scale += fabs(U(i,k)); + if (scale!=0) { + for (k=ppi;k<cols;k++) { + U(i,k) /= scale; + s += U(i,k)*U(i,k); + } + f=U(i,ppi); + g = -SIGN(e_scalar(sqrt(s)),f); + h=f*g-s; + U(i,ppi)=f-g; + 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); + for (k=ppi;k<cols;k++) U(j,k) += s*tmp(k); + } + for (k=ppi;k<cols;k++) U(i,k) *= scale; + } + } + maxarg1=anorm; + maxarg2=(fabs(S(i))+fabs(tmp(i))); + anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2; + } + /* Accumulation of right-hand transformations. */ + for (i=cols-1;i>=0;i--) { + if (i<cols-1) { + if (g) { + 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); + for (k=ppi;k<cols;k++) V(k,j) += s*V(k,i); + } + } + for (j=ppi;j<cols;j++) V(i,j)=V(j,i)=0.0; + } + V(i,i)=1.0; + g=tmp(i); + ppi=i; + } + /* Accumulation of left-hand transformations. */ + for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) { + ppi=i+1; + g=S(i); + for (j=ppi;j<cols;j++) U(i,j)=0.0; + if (g) { + g=e_scalar(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); + f=(s/U(i,i))*g; + for (k=i;k<rows;k++) U(k,j) += f*U(k,i); + } + for (j=i;j<rows;j++) U(j,i) *= g; + } else { + for (j=i;j<rows;j++) U(j,i)=0.0; + } + ++U(i,i); + } + + /* Diagonalization of the bidiagonal form. */ + for (k=cols-1;k>=0;k--) { /* Loop over singular values. */ + for (its=1;its<=maxiter;its++) { /* Loop over allowed iterations. */ + flag=true; + for (ppi=k;ppi>=0;ppi--) { /* Test for splitting. */ + nm=ppi-1; /* Note that tmp(1) is always zero. */ + if ((fabs(tmp(ppi))+anorm) == anorm) { + flag=false; + break; + } + if ((fabs(S(nm)+anorm) == anorm)) break; + } + if (flag) { + c=e_scalar(0.0); /* Cancellation of tmp(l), if l>1: */ + s=e_scalar(1.); + for (i=ppi;i<=k;i++) { + f=s*tmp(i); + tmp(i)=c*tmp(i); + if ((fabs(f)+anorm) == anorm) break; + g=S(i); + h=PYTHAG(f,g); + S(i)=h; + h=e_scalar(1.0)/h; + c=g*h; + s=(-f*h); + for (j=0;j<rows;j++) { + y=U(j,nm); + z=U(j,i); + U(j,nm)=y*c+z*s; + U(j,i)=z*c-y*s; + } + } + } + z=S(k); + + if (ppi == k) { /* Convergence. */ + if (z < e_scalar(0.0)) { /* Singular value is made nonnegative. */ + S(k) = -z; + for (j=0;j<cols;j++) V(j,k)=-V(j,k); + } + break; + } + + x=S(ppi); /* Shift from bottom 2-by-2 minor: */ + nm=k-1; + y=S(nm); + g=tmp(nm); + h=tmp(k); + f=((y-z)*(y+z)+(g-h)*(g+h))/(e_scalar(2.0)*h*y); + + g=PYTHAG(f,e_scalar(1.0)); + f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x; + + /* Next QR transformation: */ + c=s=1.0; + for (j=ppi;j<=nm;j++) { + i=j+1; + g=tmp(i); + y=S(i); + h=s*g; + g=c*g; + z=PYTHAG(f,h); + tmp(j)=z; + c=f/z; + s=h/z; + f=x*c+g*s; + g=g*c-x*s; + h=y*s; + y=y*c; + for (jj=0;jj<cols;jj++) { + x=V(jj,j); + z=V(jj,i); + V(jj,j)=x*c+z*s; + V(jj,i)=z*c-x*s; + } + z=PYTHAG(f,h); + S(j)=z; + if (z) { + z=e_scalar(1.0)/z; + c=f*z; + s=h*z; + } + f=(c*g)+(s*y); + x=(c*y)-(s*g); + for (jj=0;jj<rows;jj++) { + y=U(jj,j); + z=U(jj,i); + U(jj,j)=y*c+z*s; + U(jj,i)=z*c-y*s; + } + } + tmp(ppi)=0.0; + tmp(k)=f; + S(k)=x; + } + } + + //Sort eigen values: + for (i=0; i<cols; i++){ + + double S_max = S(i); + int i_max = i; + for (j=i+1; j<cols; j++){ + double Sj = S(j); + if (Sj > S_max){ + S_max = Sj; + i_max = j; + } + } + if (i_max != i){ + /* swap eigenvalues */ + e_scalar tmp = S(i); + S(i)=S(i_max); + S(i_max)=tmp; + + /* swap eigenvectors */ + U.col(i).swap(U.col(i_max)); + V.col(i).swap(V.col(i_max)); + } + } + + + if (its == maxiter) + return (-2); + else + return (0); + } + +} +#endif diff --git a/intern/itasc/kdl/utilities/traits.h b/intern/itasc/kdl/utilities/traits.h new file mode 100644 index 00000000000..2656d633653 --- /dev/null +++ b/intern/itasc/kdl/utilities/traits.h @@ -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 + + + diff --git a/intern/itasc/kdl/utilities/utility.cpp b/intern/itasc/kdl/utilities/utility.cpp new file mode 100644 index 00000000000..1ab9cb6f83d --- /dev/null +++ b/intern/itasc/kdl/utilities/utility.cpp @@ -0,0 +1,21 @@ +/** @file utility.cpp + * @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 accomodate doxygen + */ + +#include "utility.h" + +namespace KDL { + +int STREAMBUFFERSIZE=10000; +int MAXLENFILENAME = 255; +const double PI= 3.1415926535897932384626433832795; +const double deg2rad = 0.01745329251994329576923690768488; +const double rad2deg = 57.2957795130823208767981548141052; +double epsilon = 0.000001; +double epsilon2 = 0.000001*0.000001; +} diff --git a/intern/itasc/kdl/utilities/utility.h b/intern/itasc/kdl/utilities/utility.h new file mode 100644 index 00000000000..7151792536e --- /dev/null +++ b/intern/itasc/kdl/utilities/utility.h @@ -0,0 +1,299 @@ +/***************************************************************************** + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + * \file + * Included by most lrl-files to provide some general + * functions and macro definitions. + * + * \par history + * - changed layout of the comments to accomodate 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 + + + + + +/** + * 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 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; + +//! power or 2 of epsilon +extern double epsilon2; + +//! 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 diff --git a/intern/itasc/kdl/utilities/utility_io.cpp b/intern/itasc/kdl/utilities/utility_io.cpp new file mode 100644 index 00000000000..994567dfdfc --- /dev/null +++ b/intern/itasc/kdl/utilities/utility_io.cpp @@ -0,0 +1,208 @@ +/***************************************************************************** + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id$ + * $Name: $ + * \todo + * make IO routines more robust against the differences between DOS/UNIX end-of-line style. + ****************************************************************************/ + + +#include "utility_io.h" +#include "error.h" + +#include <stdlib.h> +#include <ctype.h> +#include <string.h> + +namespace KDL { + +// +// _functions are private functions +// + + void _check_istream(std::istream& is) + { + if ((!is.good())&&(is.eof()) ) + { + throw Error_BasicIO_File(); + } + } +// Eats until the end of the line + int _EatUntilEndOfLine( std::istream& is, int* countp=NULL) { + int ch; + int count; + count = 0; + do { + ch = is.get(); + count++; + _check_istream(is); + } while (ch!='\n'); + if (countp!=NULL) *countp = count; + return ch; +} + +// Eats until the end of the comment + int _EatUntilEndOfComment( std::istream& is, int* countp=NULL) { + int ch; + int count; + count = 0; + int prevch; + ch = 0; + do { + prevch = ch; + ch = is.get(); + count++; + _check_istream(is); + if ((prevch=='*')&&(ch=='/')) { + break; + } + } while (true); + if (countp!=NULL) *countp = count; + ch = is.get(); + return ch; +} + +// Eats space-like characters and comments +// possibly returns the number of space-like characters eaten. +int _EatSpace( std::istream& is,int* countp=NULL) { + int ch; + int count; + count=-1; + do { + _check_istream(is); + + ch = is.get(); + count++; + if (ch == '#') { + ch = _EatUntilEndOfLine(is,&count); + } + if (ch == '/') { + ch = is.get(); + if (ch == '/') { + ch = _EatUntilEndOfLine(is,&count); + } else if (ch == '*') { + ch = _EatUntilEndOfComment(is,&count); + } else { + is.putback(ch); + ch = '/'; + } + } + } while ((ch==' ')||(ch=='\n')||(ch=='\t')); + if (countp!=NULL) *countp = count; + return ch; +} + + + +// Eats whites, returns, tabs and the delim character +// Checks wether delim char. is encountered. +void Eat( std::istream& is, int delim ) +{ + int ch; + ch=_EatSpace(is); + if (ch != delim) { + throw Error_BasicIO_Exp_Delim(); + } + ch=_EatSpace(is); + is.putback(ch); +} + +// Eats whites, returns, tabs and the delim character +// Checks wether delim char. is encountered. +// EatEnd does not eat all space-like char's at the end. +void EatEnd( std::istream& is, int delim ) +{ + int ch; + ch=_EatSpace(is); + if (ch != delim) { + throw Error_BasicIO_Exp_Delim(); + } +} + + + +// For each space in descript, this routine eats whites,tabs, and newlines (at least one) +// There should be no consecutive spaces in the description. +// for each letter in descript, its reads the corresponding letter in the output +// the routine is case insensitive. + + +// Simple routine, enough for our purposes. +// works with ASCII chars +inline char Upper(char ch) +{ + /*if (('a'<=ch)&&(ch<='z')) + return (ch-'a'+'A'); + else + return ch; + */ + return toupper(ch); +} + +void Eat(std::istream& is,const char* descript) +{ + // eats whites before word + char ch; + char chdescr; + ch=_EatSpace(is); + is.putback(ch); + const char* p; + p = descript; + while ((*p)!=0) { + chdescr = (char)Upper(*p); + if (chdescr==' ') { + int count=0; + ch=_EatSpace(is,&count); + is.putback(ch); + if (count==0) { + throw Error_BasicIO_Not_A_Space(); + } + } else { + ch=(char)is.get(); + if (chdescr!=Upper(ch)) { + throw Error_BasicIO_Unexpected(); + } + } + p++; + } + +} + + + +void EatWord(std::istream& is,const char* delim,char* storage,int maxsize) +{ + int ch; + char* p; + int size; + // eat white before word + ch=_EatSpace(is); + p = storage; + size=0; + int count = 0; + while ((count==0)&&(strchr(delim,ch)==NULL)) { + *p = (char) toupper(ch); + ++p; + if (size==maxsize) { + throw Error_BasicIO_ToBig(); + } + _check_istream(is); + ++size; + //ch = is.get(); + ch =_EatSpace(is,&count); + } + *p=0; + is.putback(ch); +} + + +} diff --git a/intern/itasc/kdl/utilities/utility_io.h b/intern/itasc/kdl/utilities/utility_io.h new file mode 100644 index 00000000000..2a71ce870a3 --- /dev/null +++ b/intern/itasc/kdl/utilities/utility_io.h @@ -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$ + * $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 |