diff --git a/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.cpp b/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.cpp index e4cdcb937c57..4848143856bc 100644 --- a/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.cpp +++ b/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.cpp @@ -41,7 +41,7 @@ namespace KDL{ *this = RigidBodyInertia(m,c,Ic); } - ArticulatedBodyInertia::ArticulatedBodyInertia(const Matrix3d& M, const Matrix3d& H, const Matrix3d& I) + ArticulatedBodyInertia::ArticulatedBodyInertia(const Eigen::Matrix3d& M, const Eigen::Matrix3d& H, const Eigen::Matrix3d& I) { this->M=M; this->I=I; diff --git a/src/Mod/Robot/App/kdl_cp/chainfksolver.hpp b/src/Mod/Robot/App/kdl_cp/chainfksolver.hpp index ab52b3f8039b..ea2f333d2549 100644 --- a/src/Mod/Robot/App/kdl_cp/chainfksolver.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainfksolver.hpp @@ -48,6 +48,7 @@ namespace KDL { * * @param q_in input joint coordinates * @param p_out reference to output cartesian pose + * @param segmentNr default to -1 * * @return if < 0 something went wrong */ @@ -69,6 +70,7 @@ namespace KDL { * * @param q_in input joint coordinates (position and velocity) * @param out output cartesian coordinates (position and velocity) + * @param segmentNr default to -1 * * @return if < 0 something went wrong */ @@ -92,8 +94,9 @@ namespace KDL { * * @param q_in input joint coordinates (position, velocity and * acceleration - @param out output cartesian coordinates (position, velocity + * @param out output cartesian coordinates (position, velocity * and acceleration + * @param segmentNr default to -1 * * @return if < 0 something went wrong */ diff --git a/src/Mod/Robot/App/kdl_cp/chainidsolver.hpp b/src/Mod/Robot/App/kdl_cp/chainidsolver.hpp index fe489185c6c3..8a407def30b5 100644 --- a/src/Mod/Robot/App/kdl_cp/chainidsolver.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainidsolver.hpp @@ -41,14 +41,15 @@ namespace KDL { public: /** - * Calculate inverse dynamics, from joint positions, velocity, acceleration, external forces + * Calculate inverse dynamics, from joint positions, + * velocity, acceleration, external forces * to joint torques/forces. * * @param q input joint positions * @param q_dot input joint velocities * @param q_dotdot input joint accelerations - * - * @param torque output joint torques + * @param f_ext external forces + * @param torques output joint torques * * @return if < 0 something went wrong */ diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.hpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.hpp index b78e3d855dc5..0371d362f311 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.hpp @@ -29,10 +29,6 @@ namespace KDL * * @param chain the chain to calculate the inverse velocity * kinematics for - * @param eps if a singular value is below this value, its - * inverse is set to zero, default: 0.00001 - * @param maxiter maximum iterations for the svd calculation, - * default: 150 * */ explicit ChainIkSolverVel_pinv_givens(const Chain& chain); diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp index 3a24e99742c1..80bc45022ed5 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp @@ -54,11 +54,11 @@ namespace KDL { } - void ChainIkSolverVel_wdls::setWeightJS(const MatrixXd& Mq){ + void ChainIkSolverVel_wdls::setWeightJS(const Eigen::MatrixXd& Mq){ weight_js = Mq; } - void ChainIkSolverVel_wdls::setWeightTS(const MatrixXd& Mx){ + void ChainIkSolverVel_wdls::setWeightTS(const Eigen::MatrixXd& Mx){ weight_ts = Mx; } diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.hpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.hpp index d1c171be48c0..e1068c38065d 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.hpp @@ -108,8 +108,9 @@ namespace KDL /** * Set the joint space weighting matrix * - * @param weight_js joint space weighting symetric matrix, - * default : identity. M_q : This matrix being used as a + * weight_js joint space weighting symetric matrix, + * default : identity. + * @param Mq : This matrix being used as a * weight for the norm of the joint space speed it HAS TO BE * symmetric and positive definite. We can actually deal with * matrices containing a symmetric and positive definite block @@ -132,8 +133,9 @@ namespace KDL /** * Set the task space weighting matrix * - * @param weight_ts task space weighting symetric matrix, - * default: identity M_x : This matrix being used as a weight + * weight_ts task space weighting symetric matrix, + * default: identity + * @param Mx : This matrix being used as a weight * for the norm of the error (in terms of task space speed) it * HAS TO BE symmetric and positive definite. We can actually * deal with matrices containing a symmetric and positive diff --git a/src/Mod/Robot/App/kdl_cp/frameacc.hpp b/src/Mod/Robot/App/kdl_cp/frameacc.hpp index 6521d0cf237a..eb54cfdac405 100644 --- a/src/Mod/Robot/App/kdl_cp/frameacc.hpp +++ b/src/Mod/Robot/App/kdl_cp/frameacc.hpp @@ -260,14 +260,14 @@ class TwistAcc - +} #ifdef KDL_INLINE #include "frameacc.inl" #endif -} + diff --git a/src/Mod/Robot/App/kdl_cp/frameacc.inl b/src/Mod/Robot/App/kdl_cp/frameacc.inl index dfbdc620ed33..599cf416d712 100644 --- a/src/Mod/Robot/App/kdl_cp/frameacc.inl +++ b/src/Mod/Robot/App/kdl_cp/frameacc.inl @@ -17,7 +17,7 @@ ****************************************************************************/ - +namespace KDL { /////////////////// VectorAcc ///////////////////////////////////// @@ -596,3 +596,4 @@ bool Equal(const TwistAcc& a,const Twist& b,double eps) { Equal(a.vel,b.vel,eps) ); } +} diff --git a/src/Mod/Robot/App/kdl_cp/frames.hpp b/src/Mod/Robot/App/kdl_cp/frames.hpp index c724358c7532..9604b40849e3 100644 --- a/src/Mod/Robot/App/kdl_cp/frames.hpp +++ b/src/Mod/Robot/App/kdl_cp/frames.hpp @@ -639,9 +639,9 @@ class Frame { //! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()). inline static Frame Identity(); - //! The twist is expressed wrt the current + //! The twist \ is expressed wrt the current //! frame. This frame is integrated into an updated frame with - //! . Very simple first order integration rule. + //! \. Very simple first order integration rule. inline void Integrate(const Twist& t_this,double frequency); /* @@ -1249,6 +1249,8 @@ IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1); */ IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1); +} // namespace KDL + #ifdef KDL_INLINE // #include "vector.inl" // #include "wrench.inl" @@ -1263,7 +1265,4 @@ IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1); -} - - #endif diff --git a/src/Mod/Robot/App/kdl_cp/frames.inl b/src/Mod/Robot/App/kdl_cp/frames.inl index 156acf192f8e..d85a25935640 100644 --- a/src/Mod/Robot/App/kdl_cp/frames.inl +++ b/src/Mod/Robot/App/kdl_cp/frames.inl @@ -31,7 +31,7 @@ * */ - +namespace KDL { IMETHOD Vector::Vector(const Vector & arg) { @@ -1334,3 +1334,4 @@ IMETHOD bool operator!=(const Vector2& a,const Vector2& b) { return !operator==(a,b); } +} // namespace KDL diff --git a/src/Mod/Robot/App/kdl_cp/framevel.hpp b/src/Mod/Robot/App/kdl_cp/framevel.hpp index 8573d33f8eca..ed9c87c0a1d3 100644 --- a/src/Mod/Robot/App/kdl_cp/framevel.hpp +++ b/src/Mod/Robot/App/kdl_cp/framevel.hpp @@ -52,7 +52,7 @@ IMETHOD void posrandom(doubleVel& F) { posrandom(F.grad); } -} +} //namespace KDL template <> struct Traits { @@ -383,12 +383,12 @@ IMETHOD void posrandom(FrameVel& F) { posrandom(F.p); } +} // namespace KDL + #ifdef KDL_INLINE #include "framevel.inl" #endif -} // namespace - #endif diff --git a/src/Mod/Robot/App/kdl_cp/framevel.inl b/src/Mod/Robot/App/kdl_cp/framevel.inl index 61a43fdc2d1d..1def47fd11c7 100644 --- a/src/Mod/Robot/App/kdl_cp/framevel.inl +++ b/src/Mod/Robot/App/kdl_cp/framevel.inl @@ -16,6 +16,7 @@ * $Name: $ ****************************************************************************/ +namespace KDL { // Methods and operators related to FrameVelVel // They all delegate most of the work to RotationVelVel and VectorVelVel @@ -532,3 +533,5 @@ Twist TwistVel::GetTwist() const { Twist TwistVel::GetTwistDot() const { return Twist(vel.v,rot.v); } + +} // namespace KDL diff --git a/src/Mod/Robot/App/kdl_cp/jntarray.hpp b/src/Mod/Robot/App/kdl_cp/jntarray.hpp index e0ff388ca56f..a4b1bf8356a5 100644 --- a/src/Mod/Robot/App/kdl_cp/jntarray.hpp +++ b/src/Mod/Robot/App/kdl_cp/jntarray.hpp @@ -139,6 +139,18 @@ class MyTask : public RTT::TaskContext friend void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest); friend void Multiply(const JntArray& src,const double& factor,JntArray& dest); friend void Divide(const JntArray& src,const double& factor,JntArray& dest); + /** + * Function to multiply a KDL::Jacobian with a KDL::JntArray + * to get a KDL::Twist, it should not be used to calculate the + * forward velocity kinematics, the solver classes are built + * for this purpose. + * J*q = t + * + * @param jac J + * @param src q + * @param dest t + * @post dest == (KDL::Twist::)Zero() if 0 == src.rows() (ie src is empty) + */ friend void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest); friend void SetToZero(JntArray& array); friend bool Equal(const JntArray& src1,const JntArray& src2,double eps); diff --git a/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp b/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp index ae4f0129897f..987be53f37ac 100644 --- a/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp +++ b/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp @@ -63,7 +63,7 @@ class MyTask : public RTT::TaskContext ** use j here } }; -/endcode +\endcode */ @@ -199,9 +199,9 @@ class MyTask : public RTT::TaskContext * for this purpose. * J*q = t * - * @param jac J - * @param src q - * @param dest t + * @param src J: Inertia Matrix (6x6) + * @param vec q: Jacobian (6x1) + * @param dest t: Twist (6x1) * @post dest==Twist::Zero() if 0==src.rows() (ie src is empty) */ void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest); @@ -209,9 +209,9 @@ class MyTask : public RTT::TaskContext /** * Function to set all the values of the array to 0 * - * @param array + * @param mat */ - void SetToZero(JntSpaceInertiaMatrix& matrix); + void SetToZero(JntSpaceInertiaMatrix& mat); /** * Function to check if two matrices are the same with a diff --git a/src/Mod/Robot/App/kdl_cp/joint.hpp b/src/Mod/Robot/App/kdl_cp/joint.hpp index a188aff9551e..798ce9077551 100644 --- a/src/Mod/Robot/App/kdl_cp/joint.hpp +++ b/src/Mod/Robot/App/kdl_cp/joint.hpp @@ -80,15 +80,16 @@ namespace KDL { * Constructor of a joint. * * @param name of the joint - * @param origin the origin of the joint - * @param axis the axis of the joint - * @param scale scale between joint input and actual geometric + * @param _origin the origin of the joint + * @param _axis the axis of the joint + * @param type type of the joint + * @param _scale scale between joint input and actual geometric * movement, default: 1 - * @param offset offset between joint input and actual + * @param _offset offset between joint input and actual * geometric input, default: 0 - * @param inertia 1D inertia along the joint axis, default: 0 - * @param damping 1D damping along the joint axis, default: 0 - * @param stiffness 1D stiffness along the joint axis, + * @param _inertia 1D inertia along the joint axis, default: 0 + * @param _damping 1D damping along the joint axis, default: 0 + * @param _stiffness 1D stiffness along the joint axis, * default: 0 */ Joint(const std::string& name, const Vector& _origin, const Vector& _axis, const JointType& type, const double& _scale=1, const double& _offset=0, @@ -96,15 +97,16 @@ namespace KDL { /** * Constructor of a joint. * - * @param origin the origin of the joint - * @param axis the axis of the joint - * @param scale scale between joint input and actual geometric + * @param _origin the origin of the joint + * @param _axis the axis of the joint + * @param type type of the joint + * @param _scale scale between joint input and actual geometric * movement, default: 1 - * @param offset offset between joint input and actual + * @param _offset offset between joint input and actual * geometric input, default: 0 - * @param inertia 1D inertia along the joint axis, default: 0 - * @param damping 1D damping along the joint axis, default: 0 - * @param stiffness 1D stiffness along the joint axis, + * @param _inertia 1D inertia along the joint axis, default: 0 + * @param _damping 1D damping along the joint axis, default: 0 + * @param _stiffness 1D stiffness along the joint axis, * default: 0 */ Joint(const Vector& _origin, const Vector& _axis, const JointType& type, const double& _scale=1, const double& _offset=0, diff --git a/src/Mod/Robot/App/kdl_cp/kdl.hpp b/src/Mod/Robot/App/kdl_cp/kdl.hpp index c74ede6d7121..b02e776aacd5 100644 --- a/src/Mod/Robot/App/kdl_cp/kdl.hpp +++ b/src/Mod/Robot/App/kdl_cp/kdl.hpp @@ -21,7 +21,8 @@ // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA /** - * \mainpage KDL + * \defgroup KDL Kinematics and Dynamics Library + * \ingroup ROBOT * * This is the API reference of the * Kinematics and Dynamics @@ -32,7 +33,6 @@ * - \subpage geomprim * - \ref KinematicFamily : functionality to build kinematic chains and access their kinematic and dynamic properties, such as e.g. Forward and Inverse kinematics and dynamics. * - \ref Motion : functionality to specify motion trajectories of frames and kinematic chains, such as e.g. Trapezoidal Velocity profiles. - * - \ref KDLTK : the interface code to integrate KDL into the Orocos Real-Time Toolkit (RTT). * * **/ @@ -125,5 +125,7 @@ * Typically we use the standard S.I. units: N, m, sec. * */ + +/* This code doesn't seems to be integrated with freecad - \ref KDLTK : the interface code to integrate KDL into the Orocos Real-Time Toolkit (RTT). */ diff --git a/src/Mod/Robot/App/kdl_cp/kinfam.hpp b/src/Mod/Robot/App/kdl_cp/kinfam.hpp index e3eb12f9ab41..a9ce651a3a31 100644 --- a/src/Mod/Robot/App/kdl_cp/kinfam.hpp +++ b/src/Mod/Robot/App/kdl_cp/kinfam.hpp @@ -21,6 +21,7 @@ /** * @defgroup KinematicFamily Kinematic Families + * \ingroup KDL * @brief All classes to support kinematic families. * * The Kinematic Families classes range from the basic building blocks diff --git a/src/Mod/Robot/App/kdl_cp/motion.hpp b/src/Mod/Robot/App/kdl_cp/motion.hpp index 7d427f72df08..38bfde7fdce3 100644 --- a/src/Mod/Robot/App/kdl_cp/motion.hpp +++ b/src/Mod/Robot/App/kdl_cp/motion.hpp @@ -21,6 +21,7 @@ /** * @defgroup Motion Motion + * \ingroup KDL * @brief All classes related to the non-instantaneous motion of rigid * bodies and kinematic structures, e.g., path and trajecory definitions * and their building blocks. diff --git a/src/Mod/Robot/App/kdl_cp/rotational_interpolation.hpp b/src/Mod/Robot/App/kdl_cp/rotational_interpolation.hpp index ced4914bb671..2736c93dbff7 100644 --- a/src/Mod/Robot/App/kdl_cp/rotational_interpolation.hpp +++ b/src/Mod/Robot/App/kdl_cp/rotational_interpolation.hpp @@ -105,7 +105,7 @@ class RotationalInterpolation static RotationalInterpolation* Read(std::istream& is); /** - * virtual constructor, construction by copying .. + * virtual constructor, construction by copying. */ virtual RotationalInterpolation* Clone() const = 0; diff --git a/src/Mod/Robot/App/kdl_cp/segment.hpp b/src/Mod/Robot/App/kdl_cp/segment.hpp index 2ea46664a139..53e6285ed307 100644 --- a/src/Mod/Robot/App/kdl_cp/segment.hpp +++ b/src/Mod/Robot/App/kdl_cp/segment.hpp @@ -60,7 +60,7 @@ namespace KDL { * Joint(Joint::None) * @param f_tip frame from the end of the joint to the tip of * the segment, default: Frame::Identity() - * @param M rigid body inertia of the segment, default: Inertia::Zero() + * @param I rigid body inertia of the segment, default: Inertia::Zero() */ explicit Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); /** @@ -70,7 +70,7 @@ namespace KDL { * Joint(Joint::None) * @param f_tip frame from the end of the joint to the tip of * the segment, default: Frame::Identity() - * @param M rigid body inertia of the segment, default: Inertia::Zero() + * @param I rigid body inertia of the segment, default: Inertia::Zero() */ explicit Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); Segment(const Segment& in); diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_segment.hpp b/src/Mod/Robot/App/kdl_cp/trajectory_segment.hpp index 8d7250306af7..968b81f13742 100644 --- a/src/Mod/Robot/App/kdl_cp/trajectory_segment.hpp +++ b/src/Mod/Robot/App/kdl_cp/trajectory_segment.hpp @@ -66,12 +66,12 @@ namespace KDL { bool aggregate; public: /** - * This constructor assumes that \a geom and <_motprof> are initialised correctly. + * This constructor assumes that \a geom and \<_motprof\> are initialised correctly. */ Trajectory_Segment(Path* _geom, VelocityProfile* _motprof, bool _aggregate=true); /** - * This constructor assumes that \a geom is initialised and <_motprof> needs to be + * This constructor assumes that \a geom is initialised and \<_motprof\> needs to be * set according to \a duration. */ Trajectory_Segment(Path* _geom, VelocityProfile* _motprof, double duration, bool _aggregate=true); diff --git a/src/Mod/Robot/App/kdl_cp/treefksolver.hpp b/src/Mod/Robot/App/kdl_cp/treefksolver.hpp index 3d521d340082..bbbed69680ac 100644 --- a/src/Mod/Robot/App/kdl_cp/treefksolver.hpp +++ b/src/Mod/Robot/App/kdl_cp/treefksolver.hpp @@ -50,7 +50,7 @@ namespace KDL { * * @param q_in input joint coordinates * @param p_out reference to output cartesian pose - * + * @param segmentName * @return if < 0 something went wrong */ virtual int JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName)=0; diff --git a/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h b/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h index 63596ebc8a90..81b5ecba504d 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h +++ b/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h @@ -10,7 +10,8 @@ * generalized to the Nth derivative ! * The efficiency is not very good for high derivatives. * This could be improved by also using Rall2d - * + */ +/* template class RallNd : public Rall1d< RallNd, RallNd, double > @@ -45,15 +46,15 @@ class RallNd<1> : public Rall1d { * Als je tot 3de orde een efficiente berekening kan doen, * dan kan je tot een willekeurige orde alles efficient berekenen * 0 1 2 3 - * ==> 1 2 3 4 - * ==> 3 4 5 6 + * ==\> 1 2 3 4 + * ==\> 3 4 5 6 * 4 5 6 7 * * de aangeduide berekeningen zijn niet noodzakelijk, en er is dan niets * verniet berekend in de recursieve implementatie. * of met 2de orde over 1ste order : kan ook efficient : * 0 1 - * ==>1 2 + * ==\>1 2 * 2 3 */ // N>2: diff --git a/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.hpp b/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.hpp index 2859648b0415..e6618c6d2a1c 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.hpp +++ b/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.hpp @@ -63,7 +63,7 @@ namespace KDL * @param V matrix(nxn) * @param tmp vector n * @param maxiter defaults to 150 - * + * @param epsilon defaults to 1e-300 * @return -2 if maxiter exceeded, 0 otherwise */ int svd_eigen_HH(const MatrixXd& A,MatrixXd& U,VectorXd& S,MatrixXd& V,VectorXd& tmp,int maxiter=150,double epsilon=1e-300); diff --git a/src/Mod/Robot/App/kdl_cp/utilities/utility.cxx b/src/Mod/Robot/App/kdl_cp/utilities/utility.cxx index c7981672875c..713bdac6e4f1 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/utility.cxx +++ b/src/Mod/Robot/App/kdl_cp/utilities/utility.cxx @@ -1,4 +1,4 @@ -/** @file utility.cpp +/** @file utility.cxx * @author Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * @version * ORO_Geometry V0.2 diff --git a/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.hpp b/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.hpp index 1537791db137..344ae7f59c67 100644 --- a/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.hpp +++ b/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.hpp @@ -82,9 +82,9 @@ class VelocityProfile_TrapHalf : public VelocityProfile public: /** - * \param maxvel maximal velocity of the motion profile (positive) - * \param maxacc maximal acceleration of the motion profile (positive) - * \param starting this value is true when initial velocity is zero + * \param _maxvel maximal velocity of the motion profile (positive) + * \param _maxacc maximal acceleration of the motion profile (positive) + * \param _starting this value is true when initial velocity is zero * and ending velocity is maxvel, is false for the reverse */ VelocityProfile_TrapHalf(double _maxvel=0,double _maxacc=0,bool _starting=true);