From c23c5b7c79b033f0d8f25960ae9195dd6231f42b Mon Sep 17 00:00:00 2001 From: Azamat Shakhimardanov Date: Fri, 6 Jan 2012 13:29:42 +0100 Subject: [PATCH 2/2] added vereshchagin ID solver with corrections to original from Ruben --- orocos_kdl/CMakeLists.txt | 9 +- orocos_kdl/src/chainidsolver_vereshchagin.cpp | 515 +++++++++++++++++++++++++ orocos_kdl/src/chainidsolver_vereshchagin.hpp | 171 ++++++++ 3 files changed, 688 insertions(+), 7 deletions(-) create mode 100644 orocos_kdl/src/chainidsolver_vereshchagin.cpp create mode 100644 orocos_kdl/src/chainidsolver_vereshchagin.hpp diff --git a/orocos_kdl/CMakeLists.txt b/orocos_kdl/CMakeLists.txt index ea478d6..256c105 100644 --- a/orocos_kdl/CMakeLists.txt +++ b/orocos_kdl/CMakeLists.txt @@ -44,14 +44,9 @@ ENDIF ( NOT CMAKE_BUILD_TYPE ) SET( KDL_CFLAGS "" CACHE INTERNAL "") -#find_package(Eigen 3.0 QUIET) -#if(NOT Eigen_DIR) -#include(${PROJ_SOURCE_DIR}/config/FindEigen3.cmake) -#include_DIRECTORIES(${EIGEN3_INCLUDE_DIR}) -#endif() -find_package(Eigen 2.0 QUIET) +find_package(Eigen 3.0 QUIET) if(NOT Eigen_DIR) -include(${PROJ_SOURCE_DIR}/config/FindEigen2.cmake) +include(${PROJ_SOURCE_DIR}/config/FindEigen3.cmake) include_DIRECTORIES(${EIGEN3_INCLUDE_DIR}) endif() INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS}) diff --git a/orocos_kdl/src/chainidsolver_vereshchagin.cpp b/orocos_kdl/src/chainidsolver_vereshchagin.cpp new file mode 100644 index 0000000..a7829cb --- /dev/null +++ b/orocos_kdl/src/chainidsolver_vereshchagin.cpp @@ -0,0 +1,515 @@ +// Copyright (C) 2009 + +// Version: 1.0 +// Author: +// Maintainer: +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "chainidsolver_vereshchagin.hpp" +#include "frames_io.hpp" +#include "utilities/svd_eigen_HH.hpp" + + +namespace KDL +{ +using namespace Eigen; + +ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin(const Chain& chain_, Twist root_acc, unsigned int _nc) : +chain(chain_), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), nc(_nc), +results(ns + 1, segment_info(nc)) +{ + acc_root = root_acc; + + //Provide the necessary memory for computing the inverse of M0 + nu_sum.resize(nc); + M_0_inverse.resize(nc, nc); + Um = MatrixXd::Identity(nc, nc); + Vm = MatrixXd::Identity(nc, nc); + Sm = VectorXd::Ones(nc); + tmpm = VectorXd::Ones(nc); +} + +int ChainIdSolver_Vereshchagin::CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques) +{ + //Check sizes always + if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || torques.rows() != nj || f_ext.size() != ns) + return -1; + if (alfa.columns() != nc || beta.rows() != nc) + return -2; + //do an upward recursion for position and velocities + this->initial_upwards_sweep(q, q_dot, q_dotdot, f_ext); + //do an inward recursion for inertia, forces and constraints + this->downwards_sweep(alfa, torques); + //Solve for the constraint forces + this->constraint_calculation(beta); + //do an upward recursion to propagate the result + this->final_upwards_sweep(q_dotdot, torques); + return 0; +} + +void ChainIdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const JntArray &qdot, const JntArray &qdotdot, const Wrenches& f_ext) +{ + //if (q.rows() != nj || qdot.rows() != nj || qdotdot.rows() != nj || f_ext.size() != ns) + // return -1; + + unsigned int j = 0; + F_total = Frame::Identity(); + for (unsigned int i = 0; i < ns; i++) + { + //Express everything in the segments reference frame (body coordinates) + //which is at the segments tip, i.e. where the next joint is attached. + + //Calculate segment properties: X,S,vj,cj + const Segment& segment = chain.getSegment(i); + segment_info& s = results[i + 1]; + //The pose between the joint root and the segment tip (tip expressed in joint root coordinates) + s.F = segment.pose(q(j)); //X pose of each link in link coord system + + F_total = F_total * s.F; //X pose of the each link in root coord system + s.F_base = F_total; //X pose of the each link in root coord system for getter functions + + //The velocity due to the joint motion of the segment expressed in the segments reference frame (tip) + Twist vj = s.F.M.Inverse(segment.twist(q(j), qdot(j))); //XDot of each link + Twist aj = s.F.M.Inverse(segment.twist(q(j), qdotdot(j))); //XDotDot of each link + + //The unit velocity due to the joint motion of the segment expressed in the segments reference frame (tip) + s.Z = s.F.M.Inverse(segment.twist(q(j), 1.0)); + //Put Z in the joint root reference frame: + s.Z = s.F * s.Z; + + //The total velocity of the segment expressed in the the segments reference frame (tip) + if (i != 0) + { + s.v = s.F.Inverse(results[i].v) + vj; // recursive velocity of each link in segment frame + //s.A=s.F.Inverse(results[i].A)+aj; + s.A = s.F.M.Inverse(results[i].A); + } + else + { + s.v = vj; + s.A = s.F.M.Inverse(acc_root); + } + //c[i] = cj + v[i]xvj (remark: cj=0, since our S is not time dependent in local coordinates) + //The velocity product acceleration + //std::cout << i << " Initial upward" << s.v << std::endl; + s.C = s.v*vj; //This is a cross product: cartesian space BIAS acceleration in local link coord. + //Put C in the joint root reference frame + s.C = s.F * s.C; //+F_total.M.Inverse(acc_root)); + //The rigid body inertia of the segment, expressed in the segments reference frame (tip) + s.H = segment.getInertia(); + + //wrench of the rigid body bias forces and the external forces on the segment (in body coordinates, tip) + //external forces are taken into account through s.U. Check why s.U is given as below + Wrench FextLocal = F_total.M.Inverse() * f_ext[i]; + s.U = s.v * (s.H * s.v) - FextLocal; //f_ext[i]; + if (segment.getJoint().getType() != Joint::None) + j++; + } + +} + +void ChainIdSolver_Vereshchagin::downwards_sweep(const Jacobian& alfa, const JntArray &torques) +{ + unsigned int j = nj - 1; + for (int i = ns; i >= 0; i--) + { + //Get a handle for the segment we are working on. + segment_info& s = results[i]; + //For segment N, + //tilde is in the segment refframe (tip, not joint root) + //without tilde is at the joint root (the childs tip!!!) + //P_tilde is the articulated body inertia + //R_tilde is the sum of external and coriolis/centrifugal forces + //M is the (unit) acceleration energy already generated at link i + //G is the (unit) magnitude of the constraint forces at link i + //E are the (unit) constraint forces due to the constraints + if (i == ns) + { + s.P_tilde = s.H; + s.R_tilde = s.U; + s.M.setZero(); + s.G.setZero(); + //changeBase(alfa_N,F_total.M.Inverse(),alfa_N2); + for (unsigned int r = 0; r < 3; r++) + for (unsigned int c = 0; c < nc; c++) + { + //copy alfa constrain force matrix in E~ + s.E_tilde(r, c) = alfa(r + 3, c); + s.E_tilde(r + 3, c) = alfa(r, c); + } + //Change the reference frame of alfa to the segmentN tip frame + //F_Total holds end effector frame, if done per segment bases then constraints could be extended to all segments + Rotation base_to_end = F_total.M.Inverse(); + for (unsigned int c = 0; c < nc; c++) + { + Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)), + Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c))); + col = base_to_end*col; + s.E_tilde.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data); + } + } + else + { + //For all others: + //Everything should expressed in the body coordinates of segment i + segment_info& child = results[i + 1]; + //Copy PZ into a vector so we can do matrix manipulations, put torques above forces + Vector6d vPZ; + vPZ << Vector3d::Map(child.PZ.torque.data), Vector3d::Map(child.PZ.force.data); + Matrix6d PZDPZt = (vPZ * vPZ.transpose()).lazy(); + PZDPZt /= child.D; + + //equation a) (see Vereshchagin89) PZDPZt=[I,H;H',M] + //Azamat:articulated body inertia as in Featherstone (7.19) + s.P_tilde = s.H + child.P - ArticulatedBodyInertia(PZDPZt.corner < 3, 3 > (BottomRight), PZDPZt.corner < 3, 3 > (TopRight), PZDPZt.corner < 3, 3 > (TopLeft)); + //equation b) (see Vereshchagin89) + //Azamat: bias force as in Featherstone (7.20) + s.R_tilde = s.U + child.R + child.PC + (child.PZ / child.D) * child.u; + //equation c) (see Vereshchagin89) + s.E_tilde = child.E; + + //Azamat: equation (c) right side term + s.E_tilde -= (vPZ * child.EZ.transpose()).lazy() / child.D; + + //equation d) (see Vereshchagin89) + s.M = child.M; + //Azamat: equation (d) right side term + s.M -= (child.EZ * child.EZ.transpose()).lazy() / child.D; + + //equation e) (see Vereshchagin89) + s.G = child.G; + Twist CiZDu = child.C + (child.Z / child.D) * child.u; + Vector6d vCiZDu; + vCiZDu << Vector3d::Map(CiZDu.rot.data), Vector3d::Map(CiZDu.vel.data); + s.G += (child.E.transpose() * vCiZDu).lazy(); + } + if (i != 0) + { + //Transform all results to joint root coordinates of segment i (== body coordinates segment i-1) + //equation a) + s.P = s.F * s.P_tilde; + //equation b) + s.R = s.F * s.R_tilde; + //equation c), in matrix: torques above forces, so switch and switch back + for (unsigned int c = 0; c < nc; c++) + { + Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)), + Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c))); + col = s.F*col; + s.E.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data); + } + + //needed for next recursion + s.PZ = s.P * s.Z; + s.D = dot(s.Z, s.PZ); + s.PC = s.P * s.C; + + //u=(Q-Z(R+PC)=sum of external forces along the joint axes, + //R are the forces comming from the children, + //Q is taken zero (do we need to take the previous calculated torques? + + //projection of coriolis and centrepital forces into joint subspace (0 0 Z) + s.totalBias = -dot(s.Z, s.R + s.PC); + s.u = torques(j) + s.totalBias; + + //Matrix form of Z, put rotations above translations + Vector6d vZ; + vZ << Vector3d::Map(s.Z.rot.data), Vector3d::Map(s.Z.vel.data); + s.EZ = (s.E.transpose() * vZ).lazy(); + + if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) + j--; + } + + /* + std::cout<<"E~ "< axis torque/force + double constraint_torque = -dot(s.Z, constraint_force); + //The result should be the torque at this joint + + torques(j) = constraint_torque; + //s.constAccComp = torques(j) / s.D; + s.constAccComp = constraint_torque / s.D; + s.nullspaceAccComp = s.u / s.D; + + q_dotdot(j) = (s.nullspaceAccComp + parentAccComp + s.constAccComp); + s.acc = s.F.Inverse(a_p + s.Z * q_dotdot(j) + s.C);//returns acceleration in link distal tip coordinates. For use needs to be transformed + if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) + j++; + } +} + +/* +void ChainIdSolver_Vereshchagin::getLinkCartesianPose(Frames& x_base) +{ + for (int i = 0; i < ns; i++) + { + x_base[i] = results[i + 1].F_base; + } + return; +} + +void ChainIdSolver_Vereshchagin::getLinkCartesianVelocity(Twists& xDot_base) +{ + + for (int i = 0; i < ns; i++) + { + xDot_base[i] = results[i + 1].F_base.M * results[i + 1].v; + } + return; +} + +void ChainIdSolver_Vereshchagin::getLinkCartesianAcceleration(Twists& xDotDot_base) +{ + + for (int i = 0; i < ns; i++) + { + xDotDot_base[i] = results[i + 1].F_base.M * results[i + 1].acc; + //std::cout << "XDotDot_base[i] " << xDotDot_base[i] << std::endl; + } + return; +} + +void ChainIdSolver_Vereshchagin::getLinkPose(Frames& x_local) +{ + for (int i = 0; i < ns; i++) + { + x_local[i] = results[i + 1].F; + } + return; +} + +void ChainIdSolver_Vereshchagin::getLinkVelocity(Twists& xDot_local) +{ + for (int i = 0; i < ns; i++) + { + xDot_local[i] = results[i + 1].v; + } + return; + +} + +void ChainIdSolver_Vereshchagin::getLinkAcceleration(Twists& xDotdot_local) +{ + for (int i = 0; i < ns; i++) + { + xDotdot_local[i] = results[i + 1].acc; + } + return; + +} + +void ChainIdSolver_Vereshchagin::getJointBiasAcceleration(JntArray& bias_q_dotdot) +{ + for (int i = 0; i < ns; i++) + { + //this is only force + double tmp = results[i + 1].totalBias; + //this is accelleration + bias_q_dotdot(i) = tmp / results[i + 1].D; + + //s.totalBias = - dot(s.Z, s.R + s.PC); + //std::cout << "totalBiasAccComponent" << i << ": " << bias_q_dotdot(i) << std::endl; + //bias_q_dotdot(i) = s.totalBias/s.D + + } + return; + +} + +void ChainIdSolver_Vereshchagin::getJointConstraintAcceleration(JntArray& constraint_q_dotdot) +{ + for (int i = 0; i < ns; i++) + { + constraint_q_dotdot(i) = results[i + 1].constAccComp; + //double tmp = results[i + 1].u; + //s.u = torques(j) + s.totalBias; + // std::cout << "s.constraintAccComp" << i << ": " << results[i+1].constAccComp << std::endl; + //nullspace_q_dotdot(i) = s.u/s.D + + } + return; + + +} + +//Check the name it does not seem to be appropriate + +void ChainIdSolver_Vereshchagin::getJointNullSpaceAcceleration(JntArray& nullspace_q_dotdot) +{ + for (int i = 0; i < ns; i++) + { + nullspace_q_dotdot(i) = results[i + 1].nullspaceAccComp; + //double tmp = results[i + 1].u; + //s.u = torques(j) + s.totalBias; + //std::cout << "s.nullSpaceAccComp" << i << ": " << results[i+1].nullspaceAccComp << std::endl; + //nullspace_q_dotdot(i) = s.u/s.D + + } + return; + + +} + +//This is not only a bias force energy but also includes generalized forces +//change type of parameter G +//this method should retur array of G's + +void ChainIdSolver_Vereshchagin::getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G) +{ + for (int i = 0; i < ns; i++) + { + G = results[i + 1].G; + //double tmp = results[i + 1].u; + //s.u = torques(j) + s.totalBias; + //std::cout << "s.G " << i << ": " << results[i+1].G << std::endl; + //nullspace_q_dotdot(i) = s.u/s.D + + } + return; + +} + +//this method should retur array of R's + +void ChainIdSolver_Vereshchagin::getLinkBiasForceMatrix(Wrenches& R_tilde) +{ + for (int i = 0; i < ns; i++) + { + R_tilde[i] = results[i + 1].R_tilde; + //Azamat: bias force as in Featherstone (7.20) + //s.R_tilde = s.U + child.R + child.PC + child.PZ / child.D * child.u; + std::cout << "s.R_tilde " << i << ": " << results[i + 1].R_tilde << std::endl; + } + return; +} + +//this method should retur array of M's + +void ChainIdSolver_Vereshchagin::getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M) +{ + + +} + +//this method should retur array of E_tilde's + +void ChainIdSolver_Vereshchagin::getLinkUnitForceMatrix(Matrix6Xd& E_tilde) +{ + + +} + +*/ + +}//namespace diff --git a/orocos_kdl/src/chainidsolver_vereshchagin.hpp b/orocos_kdl/src/chainidsolver_vereshchagin.hpp new file mode 100644 index 0000000..732f346 --- /dev/null +++ b/orocos_kdl/src/chainidsolver_vereshchagin.hpp @@ -0,0 +1,171 @@ +// Copyright (C) 2009 + +// Version: 1.0 +// Author: +// Maintainer: +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP +#define KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP + +#include "chainidsolver.hpp" +#include "frames.hpp" +#include "articulatedbodyinertia.hpp" + +namespace KDL { + /** + * \brief Dynamics calculations by constraints based on Vereshchagin 1989. + * + * For a chain + */ + typedef std::vector Twists; + typedef std::vector Frames; + + typedef Eigen::Matrix Vector6d; + typedef Eigen::Matrix Matrix6d; + typedef Eigen::Matrix Matrix6Xd; + + class ChainIdSolver_Vereshchagin { + public: + /** + * Constructor for the solver, it will allocate all the necessary memory + * \param chain The kinematic chain to calculate the inverse dynamics for, an internal copy will be made. + * \param root_acc The acceleration vector of the root to use during the calculation.(most likely contains gravity) + * + */ + ChainIdSolver_Vereshchagin(const Chain& chain, Twist root_acc, unsigned int nc); + + ~ChainIdSolver_Vereshchagin() { + }; + int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques); + + /** + * Function to calculate from Cartesian forces to joint torques. + * Input parameters; + * \param q The current joint positions + * \param q_dot The current joint velocities + * \param f_ext The external forces (no gravity) on the segments + * Output parameters: + * \param q_dotdot The joint accelerations + * \param torques the resulting torques for the joints + */ + /* + + //Returns cartesian positions of links in base coordinates + void getLinkCartesianPose(Frames& x_base); + //Returns cartesian velocities of links in base coordinates + void getLinkCartesianVelocity(Twists& xDot_base); + //Returns cartesian acceleration of links in base coordinates + void getLinkCartesianAcceleration(Twists& xDotDot_base); + //Returns cartesian postions of links in link tip coordinates + void getLinkPose(Frames& x_local); + //Returns cartesian velocities of links in link tip coordinates + void getLinkVelocity(Twists& xDot_local); + //Returns cartesian acceleration of links in link tip coordinates + void getLinkAcceleration(Twists& xDotdot_local); + //Acceleration energy due to unit constraint forces at the end-effector + void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M); + //Acceleration energy due to arm configuration: bias force plus input joint torques + void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G); + + void getLinkUnitForceMatrix(Matrix6Xd& E_tilde); + + void getLinkBiasForceMatrix(Wrenches& R_tilde); + + void getJointBiasAcceleration(JntArray &bias_q_dotdot); + + void getJointConstraintAcceleration(JntArray &constraint_q_dotdot); + + void getJointNullSpaceAcceleration(JntArray &nullspace_q_dotdot); + */ +private: + //Functions to calculate velocity, propagated inertia, propagated bias forces, constraint forces and accelerations + void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext); + void downwards_sweep(const Jacobian& alfa, const JntArray& torques); + void constraint_calculation(const JntArray& beta); + void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques); + + private: + Chain chain; + unsigned int nj; + unsigned int ns; + unsigned int nc; + Twist acc_root; + Jacobian alfa_N; + Jacobian alfa_N2; + Eigen::MatrixXd M_0_inverse; + Eigen::MatrixXd Um; + Eigen::MatrixXd Vm; + JntArray beta_N; + Eigen::VectorXd nu; + Eigen::VectorXd nu_sum; + Eigen::VectorXd Sm; + Eigen::VectorXd tmpm; + Wrench qdotdot_sum; + Frame F_total; + + struct segment_info { + Frame F; //local pose with respect to previous link in segments coordinates + Frame F_base; // pose of a segment in root coordinates + Twist Z; //Unit twist + Twist v; //twist + Twist acc; //acceleration twist + Wrench U; //wrench p of the bias forces (in cartesian space) + Wrench R; //wrench p of the bias forces + Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form + Twist C; //constraint + Twist A; //constraint + ArticulatedBodyInertia H; //I (expressed in 6*6 matrix) + ArticulatedBodyInertia P; //I (expressed in 6*6 matrix) + ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix) + Wrench PZ; //vector U[i] = I_A[i]*S[i] + Wrench PC; //vector E[i] = I_A[i]*c[i] + double D; //vector D[i] = S[i]^T*U[i] + Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints + Matrix6Xd E_tilde; + Eigen::MatrixXd M; //acceleration energy already generated at link i + Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i + Eigen::VectorXd EZ; //K[i] = Etiltde'*Z + + + double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration + double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration + double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration + + double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace + double u; //vector u[i] = torques(i) - S[i]^T*(p_A[i] + I_A[i]*C[i]) in joint subspace. Azamat: In code u[i] = torques(i) - s[i].totalBias + + segment_info(unsigned int nc) { + E.resize(6, nc); + E_tilde.resize(6, nc); + G.resize(nc); + M.resize(nc, nc); + EZ.resize(nc); + E.setZero(); + E_tilde.setZero(); + M.setZero(); + G.setZero(); + EZ.setZero(); + }; + }; + + std::vector results; + + }; +} + +#endif -- 1.7.2.5