diff --git a/src/Mod/Robot/App/kdl_cp/chain.cpp b/src/Mod/Robot/App/kdl_cp/chain.cpp index d7510ad11..90c9294f1 100644 --- a/src/Mod/Robot/App/kdl_cp/chain.cpp +++ b/src/Mod/Robot/App/kdl_cp/chain.cpp @@ -25,9 +25,9 @@ namespace KDL { using namespace std; Chain::Chain(): - segments(0), nrOfJoints(0), - nrOfSegments(0) + nrOfSegments(0), + segments(0) { } diff --git a/src/Mod/Robot/App/kdl_cp/chaindynparam.cpp b/src/Mod/Robot/App/kdl_cp/chaindynparam.cpp index 8617f80cd..30bd614ba 100644 --- a/src/Mod/Robot/App/kdl_cp/chaindynparam.cpp +++ b/src/Mod/Robot/App/kdl_cp/chaindynparam.cpp @@ -27,16 +27,16 @@ namespace KDL { ChainDynParam::ChainDynParam(const Chain& _chain, Vector _grav): chain(_chain), - grav(_grav), - chainidsolver_coriolis( chain, Vector::Zero()), - chainidsolver_gravity( chain, grav), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), + grav(_grav), jntarraynull(nj), + chainidsolver_coriolis( chain, Vector::Zero()), + chainidsolver_gravity( chain, grav), wrenchnull(ns,Wrench::Zero()), - Ic(ns), X(ns), - S(ns) + S(ns), + Ic(ns) { ag=-Twist(grav,Vector::Zero()); } diff --git a/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp b/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp index bac007c5b..7a2e1d70a 100644 --- a/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp +++ b/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp @@ -57,7 +57,7 @@ namespace KDL { const Chain chain; int nr; unsigned int nj; - unsigned int ns; + unsigned int ns; Vector grav; Vector vectornull; JntArray jntarraynull; diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.cpp index 77e1634d8..0d5f84a0d 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.cpp @@ -25,7 +25,7 @@ namespace KDL { ChainIkSolverPos_NR::ChainIkSolverPos_NR(const Chain& _chain,ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver, unsigned int _maxiter, double _eps): - chain(_chain),fksolver(_fksolver),iksolver(_iksolver),delta_q(_chain.getNrOfJoints()), + chain(_chain),iksolver(_iksolver),fksolver(_fksolver),delta_q(_chain.getNrOfJoints()), maxiter(_maxiter),eps(_eps) { } diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp index ee22e2c18..888062f34 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp @@ -36,7 +36,7 @@ namespace KDL { ChainIkSolverPos_NR_JL::ChainIkSolverPos_NR_JL(const Chain& _chain, const JntArray& _q_min, const JntArray& _q_max, ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver, unsigned int _maxiter, double _eps): - chain(_chain), q_min(chain.getNrOfJoints()), q_max(chain.getNrOfJoints()), fksolver(_fksolver),iksolver(_iksolver),delta_q(_chain.getNrOfJoints()), + chain(_chain), q_min(chain.getNrOfJoints()), q_max(chain.getNrOfJoints()), iksolver(_iksolver), fksolver(_fksolver),delta_q(_chain.getNrOfJoints()), maxiter(_maxiter),eps(_eps) { q_min = _q_min; diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.cpp index fd6dbd5b5..0849110e2 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.cpp @@ -34,8 +34,8 @@ namespace KDL jac_eigen(m,n), U(MatrixXd::Identity(m,m)), V(MatrixXd::Identity(n,n)), - S(n), B(m,n), + S(n), tempi(m), tempj(m), UY(VectorXd::Zero(6)), diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.cpp index 7ea6a4ec2..3243872b1 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.cpp @@ -36,8 +36,8 @@ namespace KDL eps(_eps), maxiter(_maxiter), alpha(_alpha), - opt_pos(_opt_pos), - weights(_weights) + weights(_weights), + opt_pos(_opt_pos) { } diff --git a/src/Mod/Robot/App/kdl_cp/joint.cpp b/src/Mod/Robot/App/kdl_cp/joint.cpp index 7dbec1b16..758fa605c 100644 --- a/src/Mod/Robot/App/kdl_cp/joint.cpp +++ b/src/Mod/Robot/App/kdl_cp/joint.cpp @@ -42,7 +42,7 @@ namespace KDL { // constructor for joint along arbitrary axis, at arbitrary origin Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): - name(_name), origin(_origin), axis(_axis / _axis.Norm()), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) + name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness), axis(_axis / _axis.Norm()), origin(_origin) { if (type != RotAxis && type != TransAxis) throw joint_type_ex; @@ -55,7 +55,7 @@ namespace KDL { // constructor for joint along arbitrary axis, at arbitrary origin Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): - name("NoName"), origin(_origin), axis(_axis / _axis.Norm()), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) + name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness), axis(_axis / _axis.Norm()), origin(_origin) { if (type != RotAxis && type != TransAxis) throw joint_type_ex; diff --git a/src/Mod/Robot/App/kdl_cp/rigidbodyinertia.cpp b/src/Mod/Robot/App/kdl_cp/rigidbodyinertia.cpp index 66da9354b..904b6de7e 100644 --- a/src/Mod/Robot/App/kdl_cp/rigidbodyinertia.cpp +++ b/src/Mod/Robot/App/kdl_cp/rigidbodyinertia.cpp @@ -30,7 +30,7 @@ namespace KDL{ const static bool mhi=true; RigidBodyInertia::RigidBodyInertia(double m_,const Vector& h_,const RotationalInertia& I_,bool mhi): - m(m_),h(h_),I(I_) + m(m_),I(I_),h(h_) { } diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp b/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp index 2dd45fe1f..013242758 100644 --- a/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp +++ b/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp @@ -31,7 +31,7 @@ namespace KDL { Frame pos; public: Trajectory_Stationary(double _duration,const Frame& _pos): - pos(_pos),duration(_duration) {} + duration(_duration),pos(_pos) {} virtual double Duration() const { return duration; } diff --git a/src/Mod/Robot/App/kdl_cp/tree.cpp b/src/Mod/Robot/App/kdl_cp/tree.cpp index 92a76915c..3e8edbb49 100644 --- a/src/Mod/Robot/App/kdl_cp/tree.cpp +++ b/src/Mod/Robot/App/kdl_cp/tree.cpp @@ -26,7 +26,7 @@ namespace KDL { using namespace std; Tree::Tree(const std::string& _root_name) : - nrOfSegments(0), nrOfJoints(0),root_name(_root_name) { + nrOfJoints(0),nrOfSegments(0),root_name(_root_name) { segments.insert(make_pair(root_name, TreeElement::Root(root_name))); } diff --git a/src/Mod/Robot/App/kdl_cp/treeiksolverpos_nr_jl.cpp b/src/Mod/Robot/App/kdl_cp/treeiksolverpos_nr_jl.cpp index 6dd7a6d4b..6d341be92 100644 --- a/src/Mod/Robot/App/kdl_cp/treeiksolverpos_nr_jl.cpp +++ b/src/Mod/Robot/App/kdl_cp/treeiksolverpos_nr_jl.cpp @@ -30,7 +30,7 @@ namespace KDL { TreeFkSolverPos& _fksolver, TreeIkSolverVel& _iksolver, unsigned int _maxiter, double _eps) : tree(_tree), q_min(tree.getNrOfJoints()), q_max(tree.getNrOfJoints()), - fksolver(_fksolver), iksolver(_iksolver), delta_q(tree.getNrOfJoints()), + iksolver(_iksolver), fksolver(_fksolver), delta_q(tree.getNrOfJoints()), endpoints(_endpoints), maxiter(_maxiter), eps(_eps) { q_min = _q_min;