├── LICENSE ├── README.md ├── kdl_ros_control ├── CMakeLists.txt ├── include │ ├── kdl │ │ ├── CMakeLists.txt │ │ ├── README.txt │ │ ├── TODO.txt │ │ ├── articulatedbodyinertia.cpp │ │ ├── articulatedbodyinertia.hpp │ │ ├── chain.cpp │ │ ├── chain.hpp │ │ ├── chaindynparam.cpp │ │ ├── chaindynparam.hpp │ │ ├── chainfksolver.hpp │ │ ├── chainfksolverpos_recursive.cpp │ │ ├── chainfksolverpos_recursive.hpp │ │ ├── chainfksolvervel_recursive.cpp │ │ ├── chainfksolvervel_recursive.hpp │ │ ├── chainidsolver.hpp │ │ ├── chainidsolver_recursive_newton_euler.cpp │ │ ├── chainidsolver_recursive_newton_euler.hpp │ │ ├── chainidsolver_vereshchagin.cpp │ │ ├── chainidsolver_vereshchagin.hpp │ │ ├── chainiksolver.hpp │ │ ├── chainiksolverpos_lma.cpp │ │ ├── chainiksolverpos_lma.hpp │ │ ├── chainiksolverpos_nr.cpp │ │ ├── chainiksolverpos_nr.hpp │ │ ├── chainiksolverpos_nr_jl.cpp │ │ ├── chainiksolverpos_nr_jl.hpp │ │ ├── chainiksolvervel_pinv.cpp │ │ ├── chainiksolvervel_pinv.hpp │ │ ├── chainiksolvervel_pinv_givens.cpp │ │ ├── chainiksolvervel_pinv_givens.hpp │ │ ├── chainiksolvervel_pinv_nso.cpp │ │ ├── chainiksolvervel_pinv_nso.hpp │ │ ├── chainiksolvervel_wdls.cpp │ │ ├── chainiksolvervel_wdls.hpp │ │ ├── chainjnttojacdotsolver.cpp │ │ ├── chainjnttojacdotsolver.hpp │ │ ├── chainjnttojacsolver.cpp │ │ ├── chainjnttojacsolver.hpp │ │ ├── config.h │ │ ├── config.h.in │ │ ├── frameacc.cpp │ │ ├── frameacc.hpp │ │ ├── frameacc.inl │ │ ├── frameacc_io.hpp │ │ ├── frames.cpp │ │ ├── frames.hpp │ │ ├── frames.inl │ │ ├── frames_io.cpp │ │ ├── frames_io.hpp │ │ ├── framevel.cpp │ │ ├── framevel.hpp │ │ ├── framevel.inl │ │ ├── framevel_io.hpp │ │ ├── jacobian.cpp │ │ ├── jacobian.hpp │ │ ├── jntarray.cpp │ │ ├── jntarray.hpp │ │ ├── jntarrayacc.cpp │ │ ├── jntarrayacc.hpp │ │ ├── jntarrayvel.cpp │ │ ├── jntarrayvel.hpp │ │ ├── jntspaceinertiamatrix.cpp │ │ ├── jntspaceinertiamatrix.hpp │ │ ├── joint.cpp │ │ ├── joint.hpp │ │ ├── kdl.hpp │ │ ├── kdl.pc.in │ │ ├── kinfam.hpp │ │ ├── kinfam_io.cpp │ │ ├── kinfam_io.hpp │ │ ├── motion.hpp │ │ ├── path.cpp │ │ ├── path.hpp │ │ ├── path_circle.cpp │ │ ├── path_circle.hpp │ │ ├── path_composite.cpp │ │ ├── path_composite.hpp │ │ ├── path_cyclic_closed.cpp │ │ ├── path_cyclic_closed.hpp │ │ ├── path_line.cpp │ │ ├── path_line.hpp │ │ ├── path_point.cpp │ │ ├── path_point.hpp │ │ ├── path_roundedcomposite.cpp │ │ ├── path_roundedcomposite.hpp │ │ ├── rigidbodyinertia.cpp │ │ ├── rigidbodyinertia.hpp │ │ ├── rotational_interpolation.cpp │ │ ├── rotational_interpolation.hpp │ │ ├── rotational_interpolation_sa.cpp │ │ ├── rotational_interpolation_sa.hpp │ │ ├── rotationalinertia.cpp │ │ ├── rotationalinertia.hpp │ │ ├── segment.cpp │ │ ├── segment.hpp │ │ ├── solveri.hpp │ │ ├── stiffness.hpp │ │ ├── trajectory.cpp │ │ ├── trajectory.hpp │ │ ├── trajectory_composite.cpp │ │ ├── trajectory_composite.hpp │ │ ├── trajectory_segment.cpp │ │ ├── trajectory_segment.hpp │ │ ├── trajectory_stationary.cpp │ │ ├── trajectory_stationary.hpp │ │ ├── tree.cpp │ │ ├── tree.hpp │ │ ├── treefksolver.hpp │ │ ├── treefksolverpos_recursive.cpp │ │ ├── treefksolverpos_recursive.hpp │ │ ├── treeiksolver.hpp │ │ ├── treeiksolverpos_nr_jl.cpp │ │ ├── treeiksolverpos_nr_jl.hpp │ │ ├── treeiksolverpos_online.cpp │ │ ├── treeiksolverpos_online.hpp │ │ ├── treeiksolvervel_wdls.cpp │ │ ├── treeiksolvervel_wdls.hpp │ │ ├── treejnttojacsolver.cpp │ │ ├── treejnttojacsolver.hpp │ │ ├── utilities │ │ │ ├── error.h │ │ │ ├── error_stack.cxx │ │ │ ├── error_stack.h │ │ │ ├── header.txt │ │ │ ├── kdl-config.h │ │ │ ├── rall1d.h │ │ │ ├── rall1d_io.h │ │ │ ├── rall2d.h │ │ │ ├── rall2d_io.h │ │ │ ├── rallNd.h │ │ │ ├── svd_HH.cpp │ │ │ ├── svd_HH.hpp │ │ │ ├── svd_eigen_HH.cpp │ │ │ ├── svd_eigen_HH.hpp │ │ │ ├── svd_eigen_Macie.hpp │ │ │ ├── traits.h │ │ │ ├── utility.cxx │ │ │ ├── utility.h │ │ │ ├── utility_io.cxx │ │ │ └── utility_io.h │ │ ├── velocityprofile.cpp │ │ ├── velocityprofile.hpp │ │ ├── velocityprofile_dirac.cpp │ │ ├── velocityprofile_dirac.hpp │ │ ├── velocityprofile_rect.cpp │ │ ├── velocityprofile_rect.hpp │ │ ├── velocityprofile_spline.cpp │ │ ├── velocityprofile_spline.hpp │ │ ├── velocityprofile_trap.cpp │ │ ├── velocityprofile_trap.hpp │ │ ├── velocityprofile_traphalf.cpp │ │ └── velocityprofile_traphalf.hpp │ └── kdl_ros_control │ │ ├── alglib │ │ ├── alglibinternal.h │ │ ├── alglibmisc.h │ │ ├── ap.h │ │ ├── linalg.h │ │ ├── manual.cpp.html │ │ ├── optimization.h │ │ ├── solvers.h │ │ └── stdafx.h │ │ ├── kdl_control.h │ │ ├── kdl_object.h │ │ ├── kdl_planner.h │ │ ├── kdl_robot.h │ │ └── utils.h ├── lib │ ├── liborocos-kdl.so │ ├── liborocos-kdl.so.1.4 │ └── liborocos-kdl.so.1.4.0 ├── package.xml └── src │ ├── alglib │ ├── alglibinternal.cpp │ ├── alglibmisc.cpp │ ├── ap.cpp │ ├── dataanalysis.cpp │ ├── diffequations.cpp │ ├── fasttransforms.cpp │ ├── integration.cpp │ ├── interpolation.cpp │ ├── linalg.cpp │ ├── optimization.cpp │ ├── solvers.cpp │ ├── specialfunctions.cpp │ └── statistics.cpp │ ├── kdl_control.cpp │ ├── kdl_object.cpp │ ├── kdl_object_test.cpp │ ├── kdl_planner.cpp │ └── kdl_robot.cpp ├── models ├── cuboid │ ├── CMakeLists.txt │ ├── meshes │ │ ├── collision │ │ │ ├── cube.dae │ │ │ └── cube.stl │ │ └── visual │ │ │ ├── cube.dae │ │ │ └── cube.stl │ ├── package.xml │ └── urdf │ │ ├── cuboid.gazebo.xacro │ │ ├── cuboid.urdf.xacro │ │ └── materials.xacro ├── lbr_iiwa │ ├── .gitignore │ ├── README.md │ ├── lbr_iiwa_control │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ └── lbr_iiwa_control.yaml │ │ ├── launch │ │ │ ├── lbr_iiwa_control.launch │ │ │ ├── lbr_iiwa_effort_pos_control.perspective │ │ │ ├── lbr_iiwa_forward_pos_control.perspective │ │ │ └── lbr_iiwa_forward_vel_control.perspective │ │ └── package.xml │ ├── lbr_iiwa_description │ │ ├── CMakeLists.txt │ │ ├── gazebo │ │ │ └── gazebo.urdf.xacro │ │ ├── launch │ │ │ ├── lbr_iiwa.rviz │ │ │ ├── lbr_iiwa_online.launch │ │ │ └── lbr_iiwa_rviz.launch │ │ ├── meshes │ │ │ ├── coarse │ │ │ │ ├── link_0.stl │ │ │ │ ├── link_1.stl │ │ │ │ ├── link_2.stl │ │ │ │ ├── link_3.stl │ │ │ │ ├── link_4.stl │ │ │ │ ├── link_5.stl │ │ │ │ ├── link_6.stl │ │ │ │ └── link_7.stl │ │ │ ├── link_0.stl │ │ │ ├── link_1.stl │ │ │ ├── link_2.stl │ │ │ ├── link_3.stl │ │ │ ├── link_4.stl │ │ │ ├── link_5.stl │ │ │ ├── link_6.stl │ │ │ └── link_7.stl │ │ ├── package.xml │ │ └── urdf │ │ │ ├── lbr_iiwa.gazebo.xacro │ │ │ ├── lbr_iiwa.transmission.xacro │ │ │ ├── lbr_iiwa.urdf.xacro │ │ │ ├── lbr_iiwa.xacro │ │ │ ├── lbr_iiwa_ee.urdf.xacro │ │ │ ├── lbr_iiwa_urdf.xacro │ │ │ ├── materials.xacro │ │ │ └── utilities.xacro │ ├── lbr_iiwa_gazebo │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ └── lbr_iiwa_world.launch │ │ ├── package.xml │ │ └── worlds │ │ │ ├── lbr_iiwa.world │ │ │ ├── lbr_iiwa_object.world │ │ │ └── lbr_iiwa_object_old.world │ ├── lbr_iiwa_java │ │ ├── Client.java │ │ ├── JointTargetsListner.java │ │ └── SmartServoControl.java │ ├── lbr_iiwa_launch │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ ├── lbr_iiwa_gazebo_effort_control.launch │ │ │ ├── lbr_iiwa_gazebo_effort_pos_control.launch │ │ │ ├── lbr_iiwa_gazebo_forward_pos_control.launch │ │ │ └── lbr_iiwa_gazebo_forward_vel_control.launch │ │ └── package.xml │ └── license.dat └── planar_end_eff │ ├── CMakeLists.txt │ ├── meshes │ ├── collision │ │ └── planar_end_eff_collision.stl │ └── visual │ │ ├── planar_end_eff_visual.blend │ │ ├── planar_end_eff_visual.blend1 │ │ └── planar_end_eff_visual.stl │ ├── package.xml │ └── urdf │ ├── materials.xacro │ ├── planar_end_eff.gazebo.xacro │ └── planar_end_eff.urdf.xacro ├── nonprehensile-object-transp-ros-pkg ├── CMakeLists.txt ├── package.xml └── src │ └── nonprehensile-object-transp-iiwa_node.cpp ├── play_video_figure.png └── rodyman.png /README.md: -------------------------------------------------------------------------------- 1 | # Nonprehensile object transportation 2 | 3 | Implementation of the nonprehensile object transportation method developed in the papers: 4 | 5 | *[M. Selvaggio](http://wpage.unina.it/mario.selvaggio/index.html), [A. Garg](), [F. Ruggiero](http://www.fabioruggiero.name/web/index.php/en/), [G. Oriolo](http://www.diag.uniroma1.it/oriolo/), [B. Siciliano](http://wpage.unina.it/sicilian/) "Nonprehensile Object Transportation via Model Predictive Non-sliding Manipulation Control", in IEEE Transactions on Control System Technology, accepted, doi: 10.1109/TCST.2023.3277224.* 6 | 7 | Click [here](https://www.youtube.com/watch?v=H14NDnmpcNg) to watch the video. 8 | 9 | [![Nonprehensile object transportation](rodyman.png)](https://www.youtube.com/watch?v=H14NDnmpcNg) 10 | 11 | *[M. Selvaggio](http://wpage.unina.it/mario.selvaggio/index.html), [J. Cacace](http://wpage.unina.it/jonathan.cacace/), [C. Pacchierotti](https://team.inria.fr/rainbow/fr/team/claudio-pacchierotti/), [F. Ruggiero](http://www.fabioruggiero.name/web/index.php/en/), [P. Robuffo Giordano](https://team.inria.fr/rainbow/fr/team/prg/), "[A Shared-control Teleoperation Architecture for Nonprehensile Object Transportation](http://wpage.unina.it/mario.selvaggio/papers/tro2021.pdf)", in IEEE Transactions on Robotics, vol. 38, no. 1, pp. 569-583, Feb. 2022, doi: 10.1109/TRO.2021.3086773.* 12 | 13 | [![Nonprehensile object transportation](play_video_figure.png)](https://www.youtube.com/watch?v=5_eReIS7Ku4) 14 | 15 | ### Citing 16 | ``` 17 | @ARTICLE{SelvaggioTCST2023, 18 | author={Selvaggio, Mario and Garg, Akash and Ruggiero, Fabio and Oriolo, Giuseppe and Siciliano, Bruno}, 19 | journal={IEEE Transactions on Control Systems Technology}, 20 | title={Non-Prehensile Object Transportation via Model Predictive Non-Sliding Manipulation Control}, 21 | year={2023}, 22 | volume={}, 23 | number={}, 24 | pages={1-14}, 25 | doi={10.1109/TCST.2023.3277224}} 26 | ``` 27 | 28 | ``` 29 | @article{SelvaggioTRO2021, 30 | author={Selvaggio, Mario and Cacace, Jonathan and Pacchierotti, Claudio and Ruggiero, Fabio and Giordano, Paolo Robuffo}, 31 | journal={IEEE Transactions on Robotics}, 32 | title={A Shared-Control Teleoperation Architecture for Nonprehensile Object Transportation}, 33 | year={2022}, 34 | volume={38}, 35 | number={1}, 36 | pages={569-583}, 37 | doi={10.1109/TRO.2021.3086773} 38 | } 39 | ``` 40 | 41 | # Instructions 42 | 43 | The code was tested with Ubuntu 20.04, and ROS Noetic. Different OS and ROS versions are possible but not supported. 44 | 45 | ### Install dependencies 46 | 47 | `sudo apt-get install ros-noetic-ros-control ros-noetic-ros-controllers` 48 | 49 | ### Clone the following packages 50 | 51 | ```cd /src``` 52 | 53 | `git clone https://github.com/mrslvg/nonprehensile-object-transp.git` 54 | 55 | `git clone https://github.com/ros/geometry.git` 56 | 57 | `git clone https://github.com/ros/kdl_parser.git` 58 | 59 | ### Compile 60 | 61 | `catkin_make --only-pkg-with-deps kdl_ros_control` 62 | 63 | `catkin_make -DCATKIN_WHITELIST_PACKAGES=""` 64 | 65 | ### Run simulation 66 | 67 | `roslaunch lbr_iiwa_launch lbr_iiwa_gazebo_effort_control.launch` 68 | 69 | `rosrun nonprehensile-object-transp-iiwa nonprehensile-object-transp-iiwa_node` 70 | 71 | Press play in gazebo to start the simulation 72 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/README.txt: -------------------------------------------------------------------------------- 1 | Coding standards : 2 | ================= 3 | 4 | - Standard naming for header file and cpp file : hpp and cpp 5 | - KDL namespace and references. 6 | 7 | Directories: 8 | ============ 9 | 10 | utilities: utility classes for KDL, should not be included separately 11 | by a user 12 | 13 | experimental: preliminary code snippets. 14 | 15 | bindings: code for binding KDL with other libraries or coding-languages -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/TODO.txt: -------------------------------------------------------------------------------- 1 | /** 2 | * \brief This file contains ToDo's that do not belong in some other file. 3 | 4 | 5 | 6 | 7 | 8 | 9 | **/ 10 | 11 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/chain.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #include "chain.hpp" 23 | 24 | namespace KDL { 25 | using namespace std; 26 | 27 | Chain::Chain(): 28 | nrOfJoints(0), 29 | nrOfSegments(0), 30 | segments(0) 31 | { 32 | } 33 | 34 | Chain::Chain(const Chain& in): 35 | nrOfJoints(0), 36 | nrOfSegments(0), 37 | segments(0) 38 | { 39 | for(unsigned int i=0;iaddSegment(in.getSegment(i)); 41 | } 42 | 43 | Chain& Chain::operator=(const Chain& arg) 44 | { 45 | nrOfJoints=0; 46 | nrOfSegments=0; 47 | segments.resize(0); 48 | for(unsigned int i=0;iaddSegment(chain.getSegment(i)); 66 | } 67 | 68 | const Segment& Chain::getSegment(unsigned int nr)const 69 | { 70 | return segments[nr]; 71 | } 72 | 73 | Segment& Chain::getSegment(unsigned int nr) 74 | { 75 | return segments[nr]; 76 | } 77 | 78 | Chain::~Chain() 79 | { 80 | } 81 | 82 | } 83 | 84 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/chaindynparam.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2009 Dominick Vanthienen 2 | 3 | // Version: 1.0 4 | // Author: Dominick Vanthienen 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDLCHAINDYNPARAM_HPP 23 | #define KDLCHAINDYNPARAM_HPP 24 | 25 | #include "chainidsolver_recursive_newton_euler.hpp" 26 | #include "articulatedbodyinertia.hpp" 27 | #include "jntspaceinertiamatrix.hpp" 28 | #include 29 | 30 | namespace KDL { 31 | 32 | /** 33 | * Implementation of a method to calculate the matrices H (inertia),C(coriolis) and G(gravitation) 34 | * for the calculation torques out of the pose and derivatives. 35 | * (inverse dynamics) 36 | * 37 | * The algorithm implementation for H is based on the book "Rigid Body 38 | * Dynamics Algorithms" of Roy Featherstone, 2008 39 | * (ISBN:978-0-387-74314-1) See page 107 for the pseudo-code. 40 | * This algorithm is extended for the use of fixed joints 41 | * 42 | * It calculates the joint-space inertia matrix, given the motion of 43 | * the joints (q,qdot,qdotdot), external forces on the segments 44 | * (expressed in the segments reference frame) and the dynamical 45 | * parameters of the segments. 46 | */ 47 | class ChainDynParam : SolverI 48 | { 49 | public: 50 | ChainDynParam(const Chain& chain, Vector _grav); 51 | virtual ~ChainDynParam(); 52 | 53 | virtual int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis); 54 | virtual int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H); 55 | virtual int JntToGravity(const JntArray &q,JntArray &gravity); 56 | 57 | /// @copydoc KDL::SolverI::updateInternalDataStructures() 58 | virtual void updateInternalDataStructures(); 59 | 60 | private: 61 | const Chain& chain; 62 | int nr; 63 | unsigned int nj; 64 | unsigned int ns; 65 | Vector grav; 66 | Vector vectornull; 67 | JntArray jntarraynull; 68 | ChainIdSolver_RNE chainidsolver_coriolis; 69 | ChainIdSolver_RNE chainidsolver_gravity; 70 | std::vector wrenchnull; 71 | std::vector X; 72 | std::vector S; 73 | //std::vector I; 74 | std::vector > Ic; 75 | Wrench F; 76 | Twist ag; 77 | 78 | }; 79 | 80 | } 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/chainfksolverpos_recursive.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDLCHAINFKSOLVERPOS_RECURSIVE_HPP 23 | #define KDLCHAINFKSOLVERPOS_RECURSIVE_HPP 24 | 25 | #include "chainfksolver.hpp" 26 | 27 | namespace KDL { 28 | 29 | /** 30 | * Implementation of a recursive forward position kinematics 31 | * algorithm to calculate the position transformation from joint 32 | * space to Cartesian space of a general kinematic chain (KDL::Chain). 33 | * 34 | * @ingroup KinematicFamily 35 | */ 36 | class ChainFkSolverPos_recursive : public ChainFkSolverPos 37 | { 38 | public: 39 | ChainFkSolverPos_recursive(const Chain& chain); 40 | ~ChainFkSolverPos_recursive(); 41 | 42 | virtual int JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr=-1); 43 | virtual int JntToCart(const JntArray& q_in, std::vector& p_out, int segmentNr=-1); 44 | 45 | virtual void updateInternalDataStructures() {}; 46 | 47 | private: 48 | const Chain& chain; 49 | }; 50 | 51 | } 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/chainfksolvervel_recursive.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDL_CHAIN_FKSOLVERVEL_RECURSIVE_HPP 23 | #define KDL_CHAIN_FKSOLVERVEL_RECURSIVE_HPP 24 | 25 | #include "chainfksolver.hpp" 26 | 27 | namespace KDL 28 | { 29 | /** 30 | * Implementation of a recursive forward position and velocity 31 | * kinematics algorithm to calculate the position and velocity 32 | * transformation from joint space to Cartesian space of a general 33 | * kinematic chain (KDL::Chain). 34 | * 35 | * @ingroup KinematicFamily 36 | */ 37 | class ChainFkSolverVel_recursive : public ChainFkSolverVel 38 | { 39 | public: 40 | ChainFkSolverVel_recursive(const Chain& chain); 41 | ~ChainFkSolverVel_recursive(); 42 | 43 | virtual int JntToCart(const JntArrayVel& q_in,FrameVel& out,int segmentNr=-1); 44 | virtual int JntToCart(const JntArrayVel& q_in,std::vector& out,int segmentNr=-1); 45 | virtual void updateInternalDataStructures() {}; 46 | private: 47 | const Chain& chain; 48 | }; 49 | } 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/chainidsolver.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2009 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDL_CHAIN_IDSOLVER_HPP 23 | #define KDL_CHAIN_IDSOLVER_HPP 24 | 25 | #include "chain.hpp" 26 | #include "frames.hpp" 27 | #include "jntarray.hpp" 28 | #include "solveri.hpp" 29 | 30 | namespace KDL 31 | { 32 | 33 | typedef std::vector Wrenches; 34 | 35 | /** 36 | * \brief This abstract class encapsulates the inverse 37 | * dynamics solver for a KDL::Chain. 38 | * 39 | */ 40 | class ChainIdSolver : public KDL::SolverI 41 | { 42 | public: 43 | /** 44 | * Calculate inverse dynamics, from joint positions, velocity, acceleration, external forces 45 | * to joint torques/forces. 46 | * 47 | * @param q input joint positions 48 | * @param q_dot input joint velocities 49 | * @param q_dotdot input joint accelerations 50 | * 51 | * @param torque output joint torques 52 | * 53 | * @return if < 0 something went wrong 54 | */ 55 | virtual int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques)=0; 56 | 57 | // Need functions to return the manipulator mass, coriolis and gravity matrices - Lagrangian Formulation. 58 | }; 59 | 60 | } 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/chainidsolver_recursive_newton_euler.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2009 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDL_CHAIN_IKSOLVER_RECURSIVE_NEWTON_EULER_HPP 23 | #define KDL_CHAIN_IKSOLVER_RECURSIVE_NEWTON_EULER_HPP 24 | 25 | #include "chainidsolver.hpp" 26 | 27 | namespace KDL{ 28 | /** 29 | * \brief Recursive newton euler inverse dynamics solver 30 | * 31 | * The algorithm implementation is based on the book "Rigid Body 32 | * Dynamics Algorithms" of Roy Featherstone, 2008 33 | * (ISBN:978-0-387-74314-1) See page 96 for the pseudo-code. 34 | * 35 | * It calculates the torques for the joints, given the motion of 36 | * the joints (q,qdot,qdotdot), external forces on the segments 37 | * (expressed in the segments reference frame) and the dynamical 38 | * parameters of the segments. 39 | */ 40 | class ChainIdSolver_RNE : public ChainIdSolver{ 41 | public: 42 | /** 43 | * Constructor for the solver, it will allocate all the necessary memory 44 | * \param chain The kinematic chain to calculate the inverse dynamics for, an internal copy will be made. 45 | * \param grav The gravity vector to use during the calculation. 46 | */ 47 | ChainIdSolver_RNE(const Chain& chain,Vector grav); 48 | ~ChainIdSolver_RNE(){}; 49 | 50 | /** 51 | * Function to calculate from Cartesian forces to joint torques. 52 | * Input parameters; 53 | * \param q The current joint positions 54 | * \param q_dot The current joint velocities 55 | * \param q_dotdot The current joint accelerations 56 | * \param f_ext The external forces (no gravity) on the segments 57 | * Output parameters: 58 | * \param torques the resulting torques for the joints 59 | */ 60 | int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques); 61 | 62 | /// @copydoc KDL::SolverI::updateInternalDataStructures 63 | virtual void updateInternalDataStructures(); 64 | 65 | private: 66 | const Chain& chain; 67 | unsigned int nj; 68 | unsigned int ns; 69 | std::vector X; 70 | std::vector S; 71 | std::vector v; 72 | std::vector a; 73 | std::vector f; 74 | Twist ag; 75 | }; 76 | } 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/chainiksolverpos_nr.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #include "chainiksolverpos_nr.hpp" 23 | 24 | namespace KDL 25 | { 26 | ChainIkSolverPos_NR::ChainIkSolverPos_NR(const Chain& _chain,ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver, 27 | unsigned int _maxiter, double _eps): 28 | chain(_chain),nj (chain.getNrOfJoints()), 29 | iksolver(_iksolver),fksolver(_fksolver), 30 | delta_q(_chain.getNrOfJoints()), 31 | maxiter(_maxiter),eps(_eps) 32 | { 33 | } 34 | 35 | void ChainIkSolverPos_NR::updateInternalDataStructures() { 36 | nj = chain.getNrOfJoints(); 37 | iksolver.updateInternalDataStructures(); 38 | fksolver.updateInternalDataStructures(); 39 | delta_q.resize(nj); 40 | } 41 | 42 | int ChainIkSolverPos_NR::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out) 43 | { 44 | if (nj != chain.getNrOfJoints()) 45 | return (error = E_NOT_UP_TO_DATE); 46 | 47 | if(q_init.rows() != nj || q_out.rows() != nj) 48 | return (error = E_SIZE_MISMATCH); 49 | 50 | q_out = q_init; 51 | 52 | unsigned int i; 53 | for(i=0;i fksolver.JntToCart(q_out,f) ) 55 | return (error = E_FKSOLVERPOS_FAILED); 56 | delta_twist = diff(f,p_in); 57 | const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q); 58 | if (E_NOERROR > rc) 59 | return (error = E_IKSOLVER_FAILED); 60 | // we chose to continue if the child solver returned a positive 61 | // "error", which may simply indicate a degraded solution 62 | Add(q_out,delta_q,q_out); 63 | if(Equal(delta_twist,Twist::Zero(),eps)) 64 | // converged, but possibly with a degraded solution 65 | return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR); 66 | } 67 | return (error = E_MAX_ITERATIONS_EXCEEDED); // failed to converge 68 | } 69 | 70 | ChainIkSolverPos_NR::~ChainIkSolverPos_NR() 71 | { 72 | } 73 | 74 | const char* ChainIkSolverPos_NR::strError(const int error) const 75 | { 76 | if (E_IKSOLVER_FAILED == error) return "Child IK solver failed"; 77 | else if (E_FKSOLVERPOS_FAILED == error) return "Child FK solver failed"; 78 | else return SolverI::strError(error); 79 | } 80 | } 81 | 82 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/chainiksolvervel_pinv_givens.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | #ifndef KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP 4 | #define KDL_CHAIN_IKSOLVERVEL_PINV_GIVENS_HPP 5 | 6 | #include "chainiksolver.hpp" 7 | #include "chainjnttojacsolver.hpp" 8 | 9 | #include 10 | 11 | using namespace Eigen; 12 | 13 | namespace KDL 14 | { 15 | /** 16 | * Implementation of a inverse velocity kinematics algorithm based 17 | * on the generalize pseudo inverse to calculate the velocity 18 | * transformation from Cartesian to joint space of a general 19 | * KDL::Chain. It uses a svd-calculation based on householders 20 | * rotations. 21 | * 22 | * @ingroup KinematicFamily 23 | */ 24 | class ChainIkSolverVel_pinv_givens : public ChainIkSolverVel 25 | { 26 | public: 27 | 28 | /** 29 | * Constructor of the solver 30 | * 31 | * @param chain the chain to calculate the inverse velocity 32 | * kinematics for 33 | * @param eps if a singular value is below this value, its 34 | * inverse is set to zero, default: 0.00001 35 | * @param maxiter maximum iterations for the svd calculation, 36 | * default: 150 37 | * 38 | */ 39 | explicit ChainIkSolverVel_pinv_givens(const Chain& chain); 40 | ~ChainIkSolverVel_pinv_givens(); 41 | 42 | virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); 43 | /** 44 | * not (yet) implemented. 45 | * 46 | */ 47 | virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return (error = E_NOT_IMPLEMENTED);}; 48 | 49 | /// @copydoc KDL::SolverI::updateInternalDataStructures 50 | virtual void updateInternalDataStructures(); 51 | 52 | private: 53 | const Chain& chain; 54 | unsigned int nj; 55 | ChainJntToJacSolver jnt2jac; 56 | Jacobian jac; 57 | bool transpose,toggle; 58 | unsigned int m,n; 59 | MatrixXd jac_eigen,U,V,B; 60 | VectorXd S,tempi,tempj,UY,SUY,qdot_eigen,v_in_eigen; 61 | }; 62 | } 63 | #endif 64 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/chainjnttojacsolver.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDL_CHAINJNTTOJACSOLVER_HPP 23 | #define KDL_CHAINJNTTOJACSOLVER_HPP 24 | 25 | #include "solveri.hpp" 26 | #include "frames.hpp" 27 | #include "jacobian.hpp" 28 | #include "jntarray.hpp" 29 | #include "chain.hpp" 30 | 31 | namespace KDL 32 | { 33 | /** 34 | * @brief Class to calculate the jacobian of a general 35 | * KDL::Chain, it is used by other solvers. It should not be used 36 | * outside of KDL. 37 | * 38 | * 39 | */ 40 | 41 | class ChainJntToJacSolver : public SolverI 42 | { 43 | public: 44 | 45 | explicit ChainJntToJacSolver(const Chain& chain); 46 | virtual ~ChainJntToJacSolver(); 47 | /** 48 | * Calculate the jacobian expressed in the base frame of the 49 | * chain, with reference point at the end effector of the 50 | * *chain. The algorithm is similar to the one used in 51 | * KDL::ChainFkSolverVel_recursive 52 | * 53 | * @param q_in input joint positions 54 | * @param jac output jacobian 55 | * 56 | * @return success/error code 57 | */ 58 | virtual int JntToJac(const JntArray& q_in, Jacobian& jac, int segmentNR=-1); 59 | 60 | /** 61 | * 62 | * @param locked_joints new values for locked joints 63 | * @return success/error code 64 | */ 65 | int setLockedJoints(const std::vector locked_joints); 66 | 67 | /// @copydoc KDL::SolverI::updateInternalDataStructures 68 | virtual void updateInternalDataStructures(); 69 | 70 | private: 71 | const Chain& chain; 72 | Twist t_tmp; 73 | Frame T_tmp; 74 | std::vector locked_joints_; 75 | }; 76 | } 77 | #endif 78 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/config.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2014 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Brian Jensen 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDL_CONFIG_H 23 | #define KDL_CONFIG_H 24 | 25 | #define KDL_VERSION_MAJOR 1 26 | #define KDL_VERSION_MINOR 4 27 | #define KDL_VERSION_PATCH 0 28 | 29 | #define KDL_VERSION (KDL_VERSION_MAJOR << 16) | (KDL_VERSION_MINOR << 8) | KDL_VERSION_PATCH 30 | 31 | #define KDL_VERSION_STRING "1.4.0" 32 | 33 | //Set which version of the Tree Interface to use 34 | #define HAVE_STL_CONTAINER_INCOMPLETE_TYPES 35 | /* #undef KDL_USE_NEW_TREE_INTERFACE */ 36 | 37 | #endif //#define KDL_CONFIG_H 38 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/config.h.in: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2014 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Brian Jensen 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDL_CONFIG_H 23 | #define KDL_CONFIG_H 24 | 25 | #define KDL_VERSION_MAJOR @KDL_VERSION_MAJOR@ 26 | #define KDL_VERSION_MINOR @KDL_VERSION_MINOR@ 27 | #define KDL_VERSION_PATCH @KDL_VERSION_PATCH@ 28 | 29 | #define KDL_VERSION (KDL_VERSION_MAJOR << 16) | (KDL_VERSION_MINOR << 8) | KDL_VERSION_PATCH 30 | 31 | #define KDL_VERSION_STRING "@KDL_VERSION@" 32 | 33 | //Set which version of the Tree Interface to use 34 | #cmakedefine HAVE_STL_CONTAINER_INCOMPLETE_TYPES 35 | #cmakedefine KDL_USE_NEW_TREE_INTERFACE 36 | 37 | #endif //#define KDL_CONFIG_H 38 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/frameacc.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * \author 3 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 4 | * 5 | * \version 6 | * ORO_Geometry V0.2 7 | * 8 | * \par History 9 | * - $log$ 10 | * 11 | * \par Release 12 | * $Id: rrframes.cpp,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ 13 | * $Name: $ 14 | ****************************************************************************/ 15 | 16 | 17 | #include "frameacc.hpp" 18 | 19 | namespace KDL { 20 | 21 | #ifndef KDL_INLINE 22 | #include "frameacc.inl" 23 | #endif 24 | 25 | } 26 | 27 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/frameacc_io.hpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * \file 3 | * Defines I/O related routines to the FrameAccs classes defined in 4 | * FrameAccs.h 5 | * 6 | * \author 7 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 8 | * 9 | * \version 10 | * ORO_Geometry V0.2 11 | * 12 | * \par History 13 | * - $log$ 14 | * 15 | * \par Release 16 | * $Id: rrframes_io.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ 17 | * $Name: $ 18 | ****************************************************************************/ 19 | #ifndef RRFRAMES_IO 20 | #define RRFRAMES_IO 21 | 22 | #include "utilities/utility_io.h" 23 | #include "utilities/rall2d_io.h" 24 | 25 | #include "frames_io.hpp" 26 | #include "frameacc.hpp" 27 | 28 | namespace KDL { 29 | 30 | 31 | // Output... 32 | inline std::ostream& operator << (std::ostream& os,const VectorAcc& r) { 33 | os << "{" << r.p << "," << r.v << "," << r.dv << "}" << std::endl; 34 | return os; 35 | } 36 | 37 | inline std::ostream& operator << (std::ostream& os,const RotationAcc& r) { 38 | os << "{" << std::endl << r.R << "," << std::endl << r.w << 39 | "," << std::endl << r.dw << std::endl << "}" << std::endl; 40 | return os; 41 | } 42 | 43 | 44 | inline std::ostream& operator << (std::ostream& os,const FrameAcc& r) { 45 | os << "{" << std::endl << r.M << "," << std::endl << r.p << "}" << std::endl; 46 | return os; 47 | } 48 | inline std::ostream& operator << (std::ostream& os,const TwistAcc& r) { 49 | os << "{" << std::endl << r.vel << "," << std::endl << r.rot << std::endl << "}" << std::endl; 50 | return os; 51 | } 52 | 53 | 54 | } // namespace Frame 55 | 56 | 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/framevel.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * \author 3 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 4 | * 5 | * \version 6 | * ORO_Geometry V0.2 7 | * 8 | * \par History 9 | * - $log$ 10 | * 11 | * \par Release 12 | * $Id: rframes.cpp,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ 13 | * $Name: $ 14 | ****************************************************************************/ 15 | 16 | 17 | #include "framevel.hpp" 18 | 19 | namespace KDL { 20 | 21 | #ifndef KDL_INLINE 22 | #include "framevel.inl" 23 | #endif 24 | 25 | 26 | 27 | } 28 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/framevel_io.hpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * \file 3 | * provides I/O operations on FrameVels classes 4 | * 5 | * \author 6 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 7 | * 8 | * \version 9 | * ORO_Geometry V2 10 | * 11 | * \par History 12 | * - $log$ 13 | * 14 | * \par Release 15 | * $Id: rframes_io.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ 16 | * $Name: $ 17 | ****************************************************************************/ 18 | #ifndef KDL_FRAMESVEL_IO 19 | #define KDL_FRAMESVEL_IO 20 | 21 | #include "utilities/utility_io.h" 22 | #include "utilities/rall1d_io.h" 23 | 24 | #include "framevel_io.hpp" 25 | #include "frames_io.hpp" 26 | 27 | namespace KDL { 28 | 29 | // Output... 30 | inline std::ostream& operator << (std::ostream& os,const VectorVel& r) { 31 | os << "{" << r.p << "," << r.v << "}" << std::endl; 32 | return os; 33 | } 34 | 35 | inline std::ostream& operator << (std::ostream& os,const RotationVel& r) { 36 | os << "{" << std::endl << r.R << "," < 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDL_JACOBIAN_HPP 23 | #define KDL_JACOBIAN_HPP 24 | 25 | #include "frames.hpp" 26 | #include 27 | 28 | namespace KDL 29 | { 30 | // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) 31 | class Jacobian; 32 | bool Equal(const Jacobian& a,const Jacobian& b,double eps=epsilon); 33 | void SetToZero(Jacobian& jac); 34 | 35 | 36 | class Jacobian 37 | { 38 | public: 39 | 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 41 | Eigen::Matrix data; 42 | Jacobian(); 43 | explicit Jacobian(unsigned int nr_of_columns); 44 | Jacobian(const Jacobian& arg); 45 | 46 | ///Allocates memory for new size (can break realtime behavior) 47 | void resize(unsigned int newNrOfColumns); 48 | 49 | ///Allocates memory if size of this and argument is different 50 | Jacobian& operator=(const Jacobian& arg); 51 | 52 | bool operator ==(const Jacobian& arg)const; 53 | bool operator !=(const Jacobian& arg)const; 54 | 55 | friend bool Equal(const Jacobian& a,const Jacobian& b,double eps); 56 | 57 | 58 | ~Jacobian(); 59 | 60 | double operator()(unsigned int i,unsigned int j)const; 61 | double& operator()(unsigned int i,unsigned int j); 62 | unsigned int rows()const; 63 | unsigned int columns()const; 64 | 65 | friend void SetToZero(Jacobian& jac); 66 | 67 | friend bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest); 68 | friend bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest); 69 | friend bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest); 70 | 71 | Twist getColumn(unsigned int i) const; 72 | void setColumn(unsigned int i,const Twist& t); 73 | 74 | void changeRefPoint(const Vector& base_AB); 75 | void changeBase(const Rotation& rot); 76 | void changeRefFrame(const Frame& frame); 77 | 78 | 79 | }; 80 | 81 | bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest); 82 | bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest); 83 | bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest); 84 | 85 | 86 | } 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/jntarray.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #include "jntarray.hpp" 23 | 24 | namespace KDL 25 | { 26 | using namespace Eigen; 27 | 28 | JntArray::JntArray() 29 | { 30 | } 31 | 32 | JntArray::JntArray(unsigned int _size): 33 | data(_size) 34 | { 35 | data.setZero(); 36 | } 37 | 38 | 39 | JntArray::JntArray(const JntArray& arg): 40 | data(arg.data) 41 | { 42 | } 43 | 44 | JntArray& JntArray::operator = (const JntArray& arg) 45 | { 46 | data=arg.data; 47 | return *this; 48 | } 49 | 50 | 51 | JntArray::~JntArray() 52 | { 53 | } 54 | 55 | void JntArray::resize(unsigned int newSize) 56 | { 57 | data.conservativeResizeLike(VectorXd::Zero(newSize)); 58 | } 59 | 60 | double JntArray::operator()(unsigned int i,unsigned int j)const 61 | { 62 | assert(j==0); 63 | return data(i); 64 | } 65 | 66 | double& JntArray::operator()(unsigned int i,unsigned int j) 67 | { 68 | assert(j==0); 69 | return data(i); 70 | } 71 | 72 | unsigned int JntArray::rows()const 73 | { 74 | return data.rows(); 75 | } 76 | 77 | unsigned int JntArray::columns()const 78 | { 79 | return data.cols(); 80 | } 81 | 82 | void Add(const JntArray& src1,const JntArray& src2,JntArray& dest) 83 | { 84 | dest.data=src1.data+src2.data; 85 | } 86 | 87 | void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest) 88 | { 89 | dest.data=src1.data-src2.data; 90 | } 91 | 92 | void Multiply(const JntArray& src,const double& factor,JntArray& dest) 93 | { 94 | dest.data=factor*src.data; 95 | } 96 | 97 | void Divide(const JntArray& src,const double& factor,JntArray& dest) 98 | { 99 | dest.data=src.data/factor; 100 | } 101 | 102 | void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest) 103 | { 104 | Eigen::Matrix t=jac.data.lazyProduct(src.data); 105 | dest=Twist(Vector(t(0),t(1),t(2)),Vector(t(3),t(4),t(5))); 106 | } 107 | 108 | void SetToZero(JntArray& array) 109 | { 110 | array.data.setZero(); 111 | } 112 | 113 | bool Equal(const JntArray& src1, const JntArray& src2,double eps) 114 | { 115 | if(src1.rows()!=src2.rows()) 116 | return false; 117 | return src1.data.isApprox(src2.data,eps); 118 | } 119 | 120 | bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}; 121 | //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);}; 122 | 123 | } 124 | 125 | 126 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/jntarrayvel.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDL_JNTARRAYVEL_HPP 23 | #define KDL_JNTARRAYVEL_HPP 24 | 25 | #include "utilities/utility.h" 26 | #include "jntarray.hpp" 27 | #include "framevel.hpp" 28 | 29 | namespace KDL 30 | { 31 | // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) 32 | class JntArrayVel; 33 | bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps=epsilon); 34 | void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); 35 | void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); 36 | void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); 37 | void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); 38 | void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest); 39 | void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); 40 | void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest); 41 | void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); 42 | void SetToZero(JntArrayVel& array); 43 | 44 | 45 | class JntArrayVel 46 | { 47 | public: 48 | JntArray q; 49 | JntArray qdot; 50 | public: 51 | JntArrayVel(){}; 52 | explicit JntArrayVel(unsigned int size); 53 | JntArrayVel(const JntArray& q,const JntArray& qdot); 54 | explicit JntArrayVel(const JntArray& q); 55 | 56 | void resize(unsigned int newSize); 57 | 58 | JntArray value()const; 59 | JntArray deriv()const; 60 | 61 | friend void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); 62 | friend void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); 63 | friend void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); 64 | friend void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); 65 | friend void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest); 66 | friend void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); 67 | friend void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest); 68 | friend void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); 69 | friend void SetToZero(JntArrayVel& array); 70 | friend bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps); 71 | 72 | }; 73 | 74 | } 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/kdl.pc.in: -------------------------------------------------------------------------------- 1 | prefix=@CMAKE_INSTALL_PREFIX@ 2 | libdir=${prefix}/lib 3 | includedir=${prefix}/include 4 | 5 | Name: orocos-kdl 6 | Description: The Orocos Kinematics and Dynamics Library 7 | Requires: 8 | Version: @KDL_VERSION@ 9 | Libs: -L${libdir} -lorocos-kdl 10 | Cflags: -I${includedir} @KDL_CFLAGS@ 11 | 12 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/kinfam.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | /** 23 | * @defgroup KinematicFamily Kinematic Families 24 | * @brief All classes to support kinematic families. 25 | * 26 | * The Kinematic Families classes range from the basic building blocks 27 | * (KDL::Joint and KDL::Segment) and their interconnected kinematic 28 | * structures (KDL::Chain, KDL::Tree and KDL::Graph), to the solver 29 | * algorithms for the kinematics and dynamics of particular kinematic 30 | * families. 31 | * 32 | * A kinematic family is a set of kinematic structures that have 33 | * similar properties, such as the same interconnection topology, the same 34 | * numerical or analytical solver algorithms, etc. Different members of the 35 | * same kinematic family differ only by the concrete values of their 36 | * kinematic and dynamic properties (link lengths, mass, etc.). 37 | * 38 | * Each kinematic structure is built from one or more Segments 39 | * (KDL::Segment). A KDL::Chain is a serial connection of 40 | * these segments; a KDL:Tree is a tree-structured 41 | * interconnection; and a KDL:Graph is a kinematic structure with a 42 | * general graph topology. (The current implementation 43 | * supports only KDL::Chain.) 44 | * 45 | * A KDL::Segment contains a KDL::Joint and an offset frame ("link length", 46 | * defined by a KDL::Frame), that represents the geometric pose 47 | * between the KDL::Joint on the previous segment and its own KDL::Joint. 48 | * 49 | * A list of all the classes is available on the modules page: \ref KinFam 50 | * 51 | * 52 | */ 53 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/kinfam_io.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDL_KINFAM_IO_HPP 23 | #define KDL_KINFAM_IO_HPP 24 | 25 | #include 26 | #include 27 | 28 | #include "joint.hpp" 29 | #include "segment.hpp" 30 | #include "chain.hpp" 31 | #include "jntarray.hpp" 32 | #include "jacobian.hpp" 33 | #include "tree.hpp" 34 | #include "jntspaceinertiamatrix.hpp" 35 | 36 | namespace KDL { 37 | std::ostream& operator <<(std::ostream& os, const Joint& joint); 38 | std::istream& operator >>(std::istream& is, Joint& joint); 39 | std::ostream& operator <<(std::ostream& os, const Segment& segment); 40 | std::istream& operator >>(std::istream& is, Segment& segment); 41 | std::ostream& operator <<(std::ostream& os, const Chain& chain); 42 | std::istream& operator >>(std::istream& is, Chain& chain); 43 | 44 | std::ostream& operator <<(std::ostream& os, const Tree& tree); 45 | std::istream& operator >>(std::istream& is, Tree& tree); 46 | 47 | std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator it); 48 | 49 | std::ostream& operator <<(std::ostream& os, const JntArray& array); 50 | std::istream& operator >>(std::istream& is, JntArray& array); 51 | std::ostream& operator <<(std::ostream& os, const Jacobian& jac); 52 | std::istream& operator >>(std::istream& is, Jacobian& jac); 53 | std::ostream& operator <<(std::ostream& os, const JntSpaceInertiaMatrix& jntspaceinertiamatrix); 54 | std::istream& operator >>(std::istream& is, JntSpaceInertiaMatrix& jntspaceinertiamatrix); 55 | 56 | /* 57 | template 58 | std::ostream& operator<<(std::ostream& os, const std::vector& vec) { 59 | os << "["; 60 | for (unsigned int i = 0; i < vec.size(); i++) 61 | os << vec[i] << " "; 62 | os << "]"; 63 | return os; 64 | } 65 | ; 66 | 67 | template 68 | std::istream& operator >>(std::istream& is, std::vector& vec) { 69 | return is; 70 | } 71 | ; 72 | */ 73 | } 74 | #endif 75 | 76 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/motion.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | /** 23 | * @defgroup Motion Motion 24 | * @brief All classes related to the non-instantaneous motion of rigid 25 | * bodies and kinematic structures, e.g., path and trajecory definitions 26 | * and their building blocks. 27 | * 28 | */ 29 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/path_cyclic_closed.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path_cyclic_closed.cxx 3 | 4 | path_cyclic_closed.cxx - description 5 | ------------------- 6 | begin : Mon May 10 2004 7 | copyright : (C) 2004 Erwin Aertbelien 8 | email : erwin.aertbelien@mech.kuleuven.ac.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | /***************************************************************************** 28 | * \author 29 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 30 | * 31 | * \version 32 | * ORO_Geometry V0.2 33 | * 34 | * \par History 35 | * - $log$ 36 | * 37 | * \par Release 38 | * $Id: path_cyclic_closed.cpp,v 1.1.1.1.2.5 2003/07/24 13:26:15 psoetens Exp $ 39 | * $Name: $ 40 | ****************************************************************************/ 41 | 42 | 43 | #include "path_cyclic_closed.hpp" 44 | #include "utilities/error.h" 45 | 46 | namespace KDL { 47 | 48 | Path_Cyclic_Closed::Path_Cyclic_Closed(Path* _geom,int _times, bool _aggregate): 49 | times(_times),geom(_geom), aggregate(_aggregate) {} 50 | 51 | double Path_Cyclic_Closed::LengthToS(double length) { 52 | throw Error_MotionPlanning_Not_Applicable(); 53 | return 0; 54 | } 55 | 56 | double Path_Cyclic_Closed::PathLength(){ 57 | return geom->PathLength()*times; 58 | } 59 | 60 | Frame Path_Cyclic_Closed::Pos(double s) const { 61 | return geom->Pos( fmod(s,geom->PathLength()) ); 62 | } 63 | 64 | Twist Path_Cyclic_Closed::Vel(double s,double sd) const { 65 | return geom->Vel( fmod(s,geom->PathLength()),sd ); 66 | } 67 | 68 | Twist Path_Cyclic_Closed::Acc(double s,double sd,double sdd) const { 69 | return geom->Acc( fmod(s,geom->PathLength()),sd,sdd ); 70 | } 71 | 72 | 73 | Path_Cyclic_Closed::~Path_Cyclic_Closed() { 74 | if (aggregate) 75 | delete geom; 76 | } 77 | 78 | Path* Path_Cyclic_Closed::Clone() { 79 | return new Path_Cyclic_Closed(geom->Clone(),times, aggregate); 80 | } 81 | 82 | void Path_Cyclic_Closed::Write(std::ostream& os) { 83 | os << "CYCLIC_CLOSED[ "; 84 | os << " ";geom->Write(os);os << std::endl; 85 | os << " " << times << std::endl; 86 | os << "]" << std::endl; 87 | } 88 | 89 | } 90 | 91 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/path_cyclic_closed.hpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 path_cyclic_closed.h 3 | 4 | path_cyclic_closed.h - description 5 | ------------------- 6 | begin : Mon January 10 2005 7 | copyright : (C) 2005 Erwin Aertbelien 8 | email : erwin.aertbelien@mech.kuleuven.ac.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | 29 | /***************************************************************************** 30 | * \author 31 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 32 | * 33 | * \version 34 | * ORO_Geometry V2 35 | * 36 | * \par History 37 | * - $log$ 38 | * 39 | * \par Release 40 | * $Id: path_cyclic_closed.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $ 41 | * $Name: $ 42 | ****************************************************************************/ 43 | 44 | #ifndef KDL_MOTION_PATH_CYCLIC_CLOSED_H 45 | #define KDL_MOTION_PATH_CYCLIC_CLOSED_H 46 | 47 | #include "frames.hpp" 48 | #include "frames_io.hpp" 49 | #include "path.hpp" 50 | #include 51 | 52 | 53 | namespace KDL { 54 | 55 | /** 56 | * A Path representing a closed circular movement, 57 | * which is traversed a number of times. 58 | * @ingroup Motion 59 | */ 60 | class Path_Cyclic_Closed : public Path 61 | { 62 | int times; 63 | Path* geom; 64 | bool aggregate; 65 | public: 66 | Path_Cyclic_Closed(Path* _geom,int _times, bool _aggregate=true); 67 | virtual double LengthToS(double length); 68 | virtual double PathLength(); 69 | virtual Frame Pos(double s) const; 70 | virtual Twist Vel(double s,double sd) const; 71 | virtual Twist Acc(double s,double sd,double sdd) const; 72 | 73 | virtual void Write(std::ostream& os); 74 | static Path* Read(std::istream& is); 75 | virtual Path* Clone(); 76 | /** 77 | * gets an identifier indicating the type of this Path object 78 | */ 79 | virtual IdentifierType getIdentifier() const { 80 | return ID_CYCLIC_CLOSED; 81 | } 82 | virtual ~Path_Cyclic_Closed(); 83 | }; 84 | 85 | 86 | 87 | } 88 | 89 | 90 | #endif 91 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/path_point.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 path_point.cxx 3 | 4 | path_point.cxx - description 5 | ------------------- 6 | begin : Mon May 10 2004 7 | copyright : (C) 2004 Erwin Aertbelien 8 | email : erwin.aertbelien@mech.kuleuven.ac.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | /***************************************************************************** 28 | * \author 29 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 30 | * 31 | * \version 32 | * ORO_Geometry V0.2 33 | * 34 | * \par History 35 | * - $log$ 36 | * 37 | * \par Release 38 | * $Id: path_point.cpp,v 1.1.2.3 2003/07/24 13:40:49 psoetens Exp $ 39 | * $Name: $ 40 | ****************************************************************************/ 41 | 42 | 43 | #include "path_point.hpp" 44 | 45 | namespace KDL { 46 | 47 | Path_Point::Path_Point(const Frame& startpos) 48 | :F_base_start(startpos) 49 | { 50 | } 51 | 52 | double Path_Point::LengthToS(double length) { 53 | return length; 54 | } 55 | double Path_Point::PathLength(){ 56 | return 0; 57 | } 58 | Frame Path_Point::Pos(double s) const { 59 | return F_base_start; 60 | } 61 | 62 | Twist Path_Point::Vel(double s,double sd) const { 63 | return Twist::Zero(); 64 | } 65 | 66 | Twist Path_Point::Acc(double s,double sd,double sdd) const { 67 | return Twist::Zero(); 68 | } 69 | 70 | Path_Point::~Path_Point() { 71 | } 72 | 73 | Path* Path_Point::Clone() { 74 | return new Path_Point( F_base_start ); 75 | } 76 | 77 | void Path_Point::Write(std::ostream& os) { 78 | os << "POINT[ "<< F_base_start << "]" << std::endl; 79 | } 80 | 81 | 82 | } 83 | 84 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/path_point.hpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 path_point.h 3 | 4 | path_point.h - description 5 | ------------------- 6 | begin : Mon January 10 2005 7 | copyright : (C) 2005 Erwin Aertbelien 8 | email : erwin.aertbelien@mech.kuleuven.ac.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | 29 | /***************************************************************************** 30 | * ALTERNATIVE FOR trajectory_stationary.h/cpp 31 | * \author 32 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 33 | * 34 | * \version 35 | 36 | * ORO_Geometry V0.2 37 | * 38 | * \par History 39 | * - $log$ 40 | * 41 | * \par Release 42 | * $Id: path_point.h,v 1.1.2.3 2003/07/24 13:40:49 psoetens Exp $ 43 | * $Name: $ 44 | ****************************************************************************/ 45 | 46 | #ifndef KDL_MOTION_PATH_POINT_H 47 | #define KDL_MOTION_PATH_POINT_H 48 | 49 | #include "path.hpp" 50 | #include "rotational_interpolation.hpp" 51 | 52 | 53 | namespace KDL { 54 | 55 | 56 | 57 | /** 58 | * A Path consisting only of a point in space. 59 | * @ingroup Motion 60 | */ 61 | class Path_Point : public Path 62 | { 63 | Frame F_base_start; 64 | public: 65 | /** 66 | * Constructs a Point Path 67 | */ 68 | Path_Point(const Frame& F_base_start); 69 | double LengthToS(double length); 70 | virtual double PathLength(); 71 | virtual Frame Pos(double s) const; 72 | virtual Twist Vel(double s,double sd) const ; 73 | virtual Twist Acc(double s,double sd,double sdd) const; 74 | virtual void Write(std::ostream& os); 75 | virtual Path* Clone(); 76 | 77 | /** 78 | * gets an identifier indicating the type of this Path object 79 | */ 80 | virtual IdentifierType getIdentifier() const { 81 | return ID_POINT; 82 | } 83 | virtual ~Path_Point(); 84 | }; 85 | 86 | } 87 | 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/rotational_interpolation.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 rotational_interpolation.cxx 3 | 4 | rotational_interpolation.cxx - description 5 | ------------------- 6 | begin : Mon May 10 2004 7 | copyright : (C) 2004 Erwin Aertbelien 8 | email : erwin.aertbelien@mech.kuleuven.ac.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | /***************************************************************************** 28 | * \author 29 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 30 | * 31 | * \version 32 | * ORO_Geometry V0.2 33 | * 34 | * \par History 35 | * - $log$ 36 | * 37 | * \par Release 38 | * $Id: rotational_interpolation.cpp,v 1.1.1.1.2.3 2003/02/24 13:13:06 psoetens Exp $ 39 | * $Name: $ 40 | ****************************************************************************/ 41 | 42 | #include "utilities/error.h" 43 | #include "utilities/error_stack.h" 44 | #include "rotational_interpolation.hpp" 45 | #include "rotational_interpolation_sa.hpp" 46 | #include 47 | #include 48 | 49 | namespace KDL { 50 | 51 | using namespace std; 52 | 53 | RotationalInterpolation* RotationalInterpolation::Read(istream& is) { 54 | // auto_ptr because exception can be thrown ! 55 | IOTrace("RotationalInterpolation::Read"); 56 | char storage[64]; 57 | EatWord(is,"[",storage,sizeof(storage)); 58 | Eat(is,'['); 59 | if (strcmp(storage,"SINGLEAXIS")==0) { 60 | IOTrace("SINGLEAXIS"); 61 | EatEnd(is,']'); 62 | IOTracePop(); 63 | IOTracePop(); 64 | return new RotationalInterpolation_SingleAxis(); 65 | } else if (strcmp(storage,"THREEAXIS")==0) { 66 | IOTrace("THREEAXIS"); 67 | throw Error_Not_Implemented(); 68 | EatEnd(is,']'); 69 | IOTracePop(); 70 | IOTracePop(); 71 | return NULL; 72 | } else if (strcmp(storage,"TWOAXIS")==0) { 73 | IOTrace("TWOAXIS"); 74 | throw Error_Not_Implemented(); 75 | EatEnd(is,']'); 76 | IOTracePop(); 77 | IOTracePop(); 78 | return NULL; 79 | } else { 80 | throw Error_MotionIO_Unexpected_Traj(); 81 | } 82 | return NULL; // just to avoid the warning; 83 | } 84 | 85 | } 86 | 87 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/rotational_interpolation_sa.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 rotational_interpolation_sa.cxx 3 | 4 | rotational_interpolation_sa.cxx - description 5 | ------------------- 6 | begin : Mon May 10 2004 7 | copyright : (C) 2004 Erwin Aertbelien 8 | email : erwin.aertbelien@mech.kuleuven.ac.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | /***************************************************************************** 28 | * \author 29 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 30 | * 31 | * \version 32 | * ORO_Geometry V0.2 33 | * 34 | * \par History 35 | * - $log$ 36 | * 37 | * \par Release 38 | * $Id: rotational_interpolation_singleaxis.cpp,v 1.1.1.1.2.2 2003/02/24 13:13:06 psoetens Exp $ 39 | * $Name: $ 40 | ****************************************************************************/ 41 | 42 | 43 | #include "rotational_interpolation_sa.hpp" 44 | #include "trajectory.hpp" 45 | 46 | namespace KDL { 47 | 48 | 49 | RotationalInterpolation_SingleAxis::RotationalInterpolation_SingleAxis() 50 | {}; 51 | 52 | void RotationalInterpolation_SingleAxis::SetStartEnd(Rotation start,Rotation end) { 53 | R_base_start = start; 54 | R_base_end = end; 55 | Rotation R_start_end = R_base_start.Inverse()*R_base_end; 56 | angle = R_start_end.GetRotAngle(rot_start_end); 57 | } 58 | 59 | Rotation RotationalInterpolation_SingleAxis::Pos(double theta) const { 60 | return R_base_start* Rotation::Rot2(rot_start_end,theta); 61 | } 62 | 63 | Vector RotationalInterpolation_SingleAxis::Vel(double theta,double thetad) const { 64 | return R_base_start * ( rot_start_end*thetad ); 65 | } 66 | 67 | Vector RotationalInterpolation_SingleAxis::Acc(double theta,double thetad,double thetadd) const { 68 | return R_base_start * ( rot_start_end* thetadd); 69 | } 70 | 71 | double RotationalInterpolation_SingleAxis::Angle() { 72 | return angle; 73 | } 74 | 75 | void RotationalInterpolation_SingleAxis::Write(std::ostream& os) const { 76 | os << "SingleAxis[] " << std::endl; 77 | } 78 | 79 | RotationalInterpolation_SingleAxis::~RotationalInterpolation_SingleAxis() { 80 | } 81 | 82 | 83 | RotationalInterpolation* RotationalInterpolation_SingleAxis::Clone() const { 84 | return new RotationalInterpolation_SingleAxis(); 85 | } 86 | 87 | } 88 | 89 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/rotational_interpolation_sa.hpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 rotational_interpolation_sa.h 3 | 4 | rotational_interpolation_sa.h - description 5 | ------------------- 6 | begin : Mon January 10 2005 7 | copyright : (C) 2005 Erwin Aertbelien 8 | email : erwin.aertbelien@mech.kuleuven.ac.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | 29 | /***************************************************************************** 30 | * \author 31 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 32 | * 33 | * \version 34 | * ORO_Geometry V0.2 35 | * 36 | * \par History 37 | * - $log$ 38 | * 39 | * \par Release 40 | * $Id: rotational_interpolation_singleaxis.h,v 1.1.1.1.2.3 2003/07/24 13:26:15 psoetens Exp $ 41 | * $Name: $ 42 | ****************************************************************************/ 43 | 44 | #ifndef KDL_ROTATIONALINTERPOLATION_SINGLEAXIS_H 45 | #define KDL_ROTATIONALINTERPOLATION_SINGLEAXIS_H 46 | 47 | #include "frames.hpp" 48 | #include "frames_io.hpp" 49 | #include "rotational_interpolation.hpp" 50 | 51 | 52 | namespace KDL { 53 | 54 | 55 | /** 56 | * An interpolation algorithm which rotates a frame over the existing 57 | * single rotation axis 58 | * formed by start and end rotation. If more than one rotational axis 59 | * exist, an arbitrary one will be chosen, therefore it is not recommended 60 | * to try to interpolate a 180 degrees rotation. 61 | * @ingroup Motion 62 | */ 63 | class RotationalInterpolation_SingleAxis: public RotationalInterpolation 64 | { 65 | Rotation R_base_start; 66 | Rotation R_base_end; 67 | Vector rot_start_end; 68 | double angle; 69 | public: 70 | RotationalInterpolation_SingleAxis(); 71 | virtual void SetStartEnd(Rotation start,Rotation end); 72 | virtual double Angle(); 73 | virtual Rotation Pos(double th) const; 74 | virtual Vector Vel(double th,double thd) const; 75 | virtual Vector Acc(double th,double thd,double thdd) const; 76 | virtual void Write(std::ostream& os) const; 77 | virtual RotationalInterpolation* Clone() const; 78 | virtual ~RotationalInterpolation_SingleAxis(); 79 | }; 80 | 81 | } 82 | 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/rotationalinertia.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2009 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #include "rotationalinertia.hpp" 23 | #include 24 | using namespace Eigen; 25 | 26 | namespace KDL 27 | { 28 | RotationalInertia::RotationalInertia(double Ixx,double Iyy,double Izz,double Ixy,double Ixz,double Iyz) 29 | { 30 | data[0]=Ixx; 31 | data[1]=data[3]=Ixy; 32 | data[2]=data[6]=Ixz; 33 | data[4]=Iyy; 34 | data[5]=data[7]=Iyz; 35 | data[8]=Izz; 36 | 37 | } 38 | 39 | RotationalInertia::~RotationalInertia() 40 | { 41 | } 42 | 43 | Vector RotationalInertia::operator*(const Vector& omega) const { 44 | // Complexity : 9M+6A 45 | Vector result; 46 | Map(result.data)=Map(this->data)*Map(omega.data); 47 | return result; 48 | } 49 | 50 | RotationalInertia operator*(double a, const RotationalInertia& I){ 51 | RotationalInertia result; 52 | Map(result.data)=a*Map(I.data); 53 | return result; 54 | } 55 | 56 | RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib){ 57 | RotationalInertia result; 58 | Map(result.data)=Map(Ia.data)+Map(Ib.data); 59 | return result; 60 | } 61 | } 62 | 63 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/rotationalinertia.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2009 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDL_ROTATIONALINERTIA_HPP 23 | #define KDL_ROTATIONALINERTIA_HPP 24 | 25 | #include "frames.hpp" 26 | 27 | //------- class for only the Rotational Inertia -------- 28 | 29 | namespace KDL 30 | { 31 | //Forward declaration 32 | class RigidBodyInertia; 33 | 34 | class RotationalInertia{ 35 | public: 36 | 37 | explicit RotationalInertia(double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0); 38 | 39 | static inline RotationalInertia Zero(){ 40 | return RotationalInertia(0,0,0,0,0,0); 41 | }; 42 | 43 | friend RotationalInertia operator*(double a, const RotationalInertia& I); 44 | friend RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib); 45 | 46 | /** 47 | * This function calculates the angular momentum resulting from a rotational velocity omega 48 | */ 49 | KDL::Vector operator*(const KDL::Vector& omega) const; 50 | 51 | ~RotationalInertia(); 52 | 53 | friend class RigidBodyInertia; 54 | ///Scalar product 55 | friend RigidBodyInertia operator*(double a,const RigidBodyInertia& I); 56 | ///addition 57 | friend RigidBodyInertia operator+(const RigidBodyInertia& Ia,const RigidBodyInertia& Ib); 58 | ///calculate spatial momentum 59 | friend Wrench operator*(const RigidBodyInertia& I,const Twist& t); 60 | ///coordinate system transform Ia = T_a_b*Ib with T_a_b the frame from a to b 61 | friend RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I); 62 | ///base frame orientation change Ia = R_a_b*Ib with R_a_b the rotation for frame from a to b 63 | friend RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I); 64 | 65 | double data[9]; 66 | }; 67 | 68 | RotationalInertia operator*(double a, const RotationalInertia& I); 69 | RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib); 70 | 71 | } 72 | 73 | #endif 74 | 75 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/segment.cpp: -------------------------------------------------------------------------------- 1 | // Version: 1.0 2 | // Author: Ruben Smits 3 | // Maintainer: Ruben Smits 4 | // URL: http://www.orocos.org/kdl 5 | 6 | // This library is free software; you can redistribute it and/or 7 | // modify it under the terms of the GNU Lesser General Public 8 | // License as published by the Free Software Foundation; either 9 | // version 2.1 of the License, or (at your option) any later version. 10 | 11 | // This library is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | // Lesser General Public License for more details. 15 | 16 | // You should have received a copy of the GNU Lesser General Public 17 | // License along with this library; if not, write to the Free Software 18 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | #include "segment.hpp" 21 | 22 | namespace KDL { 23 | 24 | Segment::Segment(const std::string& _name, const Joint& _joint, const Frame& _f_tip, const RigidBodyInertia& _I): 25 | name(_name), 26 | joint(_joint),I(_I), 27 | f_tip(_joint.pose(0).Inverse() * _f_tip) 28 | { 29 | } 30 | 31 | Segment::Segment(const Joint& _joint, const Frame& _f_tip, const RigidBodyInertia& _I): 32 | name("NoName"), 33 | joint(_joint),I(_I), 34 | f_tip(_joint.pose(0).Inverse() * _f_tip) 35 | { 36 | } 37 | 38 | Segment::Segment(const Segment& in): 39 | name(in.name),joint(in.joint),I(in.I), 40 | f_tip(in.f_tip) 41 | { 42 | } 43 | 44 | Segment& Segment::operator=(const Segment& arg) 45 | { 46 | name=arg.name; 47 | joint=arg.joint; 48 | I=arg.I; 49 | f_tip=arg.f_tip; 50 | return *this; 51 | } 52 | 53 | Segment::~Segment() 54 | { 55 | } 56 | 57 | Frame Segment::pose(const double& q)const 58 | { 59 | return joint.pose(q)*f_tip; 60 | } 61 | 62 | Twist Segment::twist(const double& q, const double& qdot)const 63 | { 64 | return joint.twist(qdot).RefPoint(joint.pose(q).M * f_tip.p); 65 | } 66 | 67 | void Segment::setFrameToTip(const Frame& f_tip_new) 68 | { 69 | f_tip = joint.pose(0).Inverse() * f_tip_new; 70 | } 71 | 72 | }//end of namespace KDL 73 | 74 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/stiffness.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDL_STIFFNESS_H 23 | #define KDL_STIFFNESS_H 24 | #include "frames.hpp" 25 | 26 | 27 | namespace KDL { 28 | /** 29 | * Preliminary class to implement Stiffness, only diagonal stiffness is implemented 30 | * no transformations provided... 31 | * 32 | * Implements a diagonal stiffness matrix. 33 | * first 3 elements are stiffness for translations 34 | * last 3 elements are stiffness for rotations. 35 | */ 36 | class Stiffness { 37 | double data[6]; 38 | public: 39 | Stiffness() { 40 | data[0]=0; 41 | data[1]=0; 42 | data[2]=0; 43 | data[3]=0; 44 | data[4]=0; 45 | data[5]=0; 46 | } 47 | Stiffness(double* d) { 48 | data[0]=d[0]; 49 | data[1]=d[1]; 50 | data[2]=d[2]; 51 | data[3]=d[3]; 52 | data[4]=d[4]; 53 | data[5]=d[5]; 54 | } 55 | Stiffness(double x,double y,double z,double rx,double ry,double rz) { 56 | data[0]=x; 57 | data[1]=y; 58 | data[2]=z; 59 | data[3]=rx; 60 | data[4]=ry; 61 | data[5]=rz; 62 | } 63 | double& operator[](int i) { 64 | return data[i]; 65 | } 66 | double operator[](int i) const { 67 | return data[i]; 68 | } 69 | Twist Inverse(const Wrench& w) const{ 70 | Twist t; 71 | t[0]=w[0]/data[0]; 72 | t[1]=w[1]/data[1]; 73 | t[2]=w[2]/data[2]; 74 | t[3]=w[3]/data[3]; 75 | t[4]=w[4]/data[4]; 76 | t[5]=w[5]/data[5]; 77 | return t; 78 | } 79 | }; 80 | 81 | inline Wrench operator * (const Stiffness& s, const Twist& t) { 82 | Wrench w; 83 | w[0]=s[0]*t[0]; 84 | w[1]=s[1]*t[1]; 85 | w[2]=s[2]*t[2]; 86 | w[3]=s[3]*t[3]; 87 | w[4]=s[4]*t[4]; 88 | w[5]=s[5]*t[5]; 89 | return w; 90 | } 91 | 92 | inline Stiffness operator+(const Stiffness& s1, const Stiffness& s2) { 93 | Stiffness s; 94 | s[0]=s1[0]+s2[0]; 95 | s[1]=s1[1]+s2[1]; 96 | s[2]=s1[2]+s2[2]; 97 | s[3]=s1[3]+s2[3]; 98 | s[4]=s1[4]+s2[4]; 99 | s[5]=s1[5]+s2[5]; 100 | return s; 101 | } 102 | inline void posrandom(Stiffness& F) { 103 | posrandom(F[0]); 104 | posrandom(F[1]); 105 | posrandom(F[2]); 106 | posrandom(F[3]); 107 | posrandom(F[4]); 108 | posrandom(F[5]); 109 | } 110 | 111 | inline void random(Stiffness& F) { 112 | posrandom(F); 113 | } 114 | 115 | 116 | } 117 | #endif 118 | 119 | 120 | 121 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/trajectory.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 trajectory.cxx 3 | 4 | trajectory.cxx - description 5 | ------------------- 6 | begin : Mon May 10 2004 7 | copyright : (C) 2004 Erwin Aertbelien 8 | email : erwin.aertbelien@mech.kuleuven.ac.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | /***************************************************************************** 28 | * \author 29 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 30 | * 31 | * \version 32 | * ORO_Geometry V0.2 33 | * 34 | * \par History 35 | * - $log$ 36 | * 37 | * \par Release 38 | * $Id: trajectory.cpp,v 1.1.1.1.2.4 2003/07/18 14:58:36 psoetens Exp $ 39 | * $Name: $ 40 | ****************************************************************************/ 41 | 42 | 43 | #include "utilities/error.h" 44 | #include "utilities/error_stack.h" 45 | #include "trajectory.hpp" 46 | #include "path.hpp" 47 | #include "trajectory_segment.hpp" 48 | 49 | #include 50 | #include 51 | 52 | 53 | namespace KDL { 54 | 55 | using namespace std; 56 | 57 | Trajectory* Trajectory::Read(std::istream& is) { 58 | // auto_ptr because exception can be thrown ! 59 | IOTrace("Trajectory::Read"); 60 | char storage[64]; 61 | EatWord(is,"[",storage,sizeof(storage)); 62 | Eat(is,'['); 63 | if (strcmp(storage,"SEGMENT")==0) { 64 | IOTrace("SEGMENT"); 65 | auto_ptr geom( Path::Read(is) ); 66 | auto_ptr motprof( VelocityProfile::Read(is) ); 67 | EatEnd(is,']'); 68 | IOTracePop(); 69 | IOTracePop(); 70 | return new Trajectory_Segment(geom.release(),motprof.release()); 71 | } else { 72 | throw Error_MotionIO_Unexpected_Traj(); 73 | } 74 | return NULL; // just to avoid the warning; 75 | } 76 | 77 | 78 | 79 | } 80 | 81 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/trajectory_composite.hpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * \author 3 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 4 | * 5 | * \version 6 | * LRL V0.2 7 | * 8 | * \par History 9 | * - $log$ 10 | * 11 | * \par Release 12 | * $Id: trajectory_composite.h 22 2004-09-21 08:58:54Z eaertbellocal $ 13 | * $Name: $ 14 | ****************************************************************************/ 15 | 16 | 17 | #ifndef TRAJECTORY_COMPOSITE_H 18 | #define TRAJECTORY_COMPOSITE_H 19 | 20 | #include "trajectory.hpp" 21 | #include "path_composite.hpp" 22 | #include 23 | 24 | 25 | 26 | namespace KDL { 27 | /** 28 | * Trajectory_Composite implements a trajectory that is composed 29 | * of underlying trajectoria. Call Add to add a trajectory 30 | * @ingroup Motion 31 | */ 32 | class Trajectory_Composite: public Trajectory 33 | { 34 | typedef std::vector VectorTraj; 35 | typedef std::vector VectorDouble; 36 | VectorTraj vt; // contains the element Trajectories 37 | VectorDouble vd; // contains end time for each Trajectory 38 | double duration; // total duration of the composed 39 | // Trajectory 40 | 41 | public: 42 | Trajectory_Composite(); 43 | // Constructs an empty composite 44 | 45 | virtual double Duration() const; 46 | virtual Frame Pos(double time) const; 47 | virtual Twist Vel(double time) const; 48 | virtual Twist Acc(double time) const; 49 | 50 | virtual void Add(Trajectory* elem); 51 | // Adds trajectory to the end of the sequence. 52 | 53 | virtual void Destroy(); 54 | virtual void Write(std::ostream& os) const; 55 | virtual Trajectory* Clone() const; 56 | 57 | virtual ~Trajectory_Composite(); 58 | }; 59 | 60 | 61 | } 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/trajectory_stationary.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * \author 3 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 4 | * 5 | * \version 6 | * LRL V0.2 7 | * 8 | * \par History 9 | * - $log$ 10 | * 11 | * \par Release 12 | * $Id: trajectory_stationary.cpp 22 2004-09-21 08:58:54Z eaertbellocal $ 13 | * $Name: $ 14 | ****************************************************************************/ 15 | 16 | 17 | #include "trajectory_stationary.hpp" 18 | 19 | namespace KDL { 20 | 21 | using namespace std; 22 | 23 | 24 | void Trajectory_Stationary::Write(ostream& os) const { 25 | os << "STATIONARY[ " << duration << endl; 26 | os << pos << endl; 27 | os << "]"; 28 | } 29 | 30 | 31 | } 32 | 33 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/trajectory_stationary.hpp: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * \author 3 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 4 | * 5 | * \version 6 | * LRL V0.2 7 | * 8 | * \par History 9 | * - $log$ 10 | * 11 | * \par Release 12 | * $Id: trajectory_stationary.h 22 2004-09-21 08:58:54Z eaertbellocal $ 13 | * $Name: $ 14 | ****************************************************************************/ 15 | 16 | #ifndef TRAJECTORY_STATIONARY_H 17 | #define TRAJECTORY_STATIONARY_H 18 | 19 | #include "trajectory.hpp" 20 | 21 | 22 | namespace KDL { 23 | /** 24 | * Implements a "trajectory" of a stationary position 25 | * for an amount of time. 26 | * @ingroup Motion 27 | */ 28 | class Trajectory_Stationary : public Trajectory 29 | { 30 | double duration; 31 | Frame pos; 32 | public: 33 | Trajectory_Stationary(double _duration,const Frame& _pos): 34 | duration(_duration),pos(_pos) {} 35 | virtual double Duration() const { 36 | return duration; 37 | } 38 | virtual Frame Pos(double time) const { 39 | return pos; 40 | } 41 | virtual Twist Vel(double time) const { 42 | return Twist::Zero(); 43 | } 44 | virtual Twist Acc(double time) const { 45 | return Twist::Zero(); 46 | } 47 | virtual void Write(std::ostream& os) const; 48 | 49 | virtual Trajectory* Clone() const { 50 | return new Trajectory_Stationary(duration,pos); 51 | } 52 | virtual ~Trajectory_Stationary() {} 53 | }; 54 | 55 | 56 | } 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/treefksolverpos_recursive.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | // Copyright (C) 2008 Julia Jesse 3 | 4 | // Version: 1.0 5 | // Author: Ruben Smits 6 | // Maintainer: Ruben Smits 7 | // URL: http://www.orocos.org/kdl 8 | 9 | // This library is free software; you can redistribute it and/or 10 | // modify it under the terms of the GNU Lesser General Public 11 | // License as published by the Free Software Foundation; either 12 | // version 2.1 of the License, or (at your option) any later version. 13 | 14 | // This library is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 | // Lesser General Public License for more details. 18 | 19 | // You should have received a copy of the GNU Lesser General Public 20 | // License along with this library; if not, write to the Free Software 21 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 22 | 23 | #include "treefksolverpos_recursive.hpp" 24 | #include 25 | 26 | namespace KDL { 27 | 28 | TreeFkSolverPos_recursive::TreeFkSolverPos_recursive(const Tree& _tree): 29 | tree(_tree) 30 | { 31 | } 32 | 33 | int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName) 34 | { 35 | SegmentMap::const_iterator it = tree.getSegment(segmentName); 36 | 37 | 38 | if(q_in.rows() != tree.getNrOfJoints()) 39 | return -1; 40 | else if(it == tree.getSegments().end()) //if the segment name is not found 41 | return -2; 42 | else{ 43 | p_out = recursiveFk(q_in, it); 44 | return 0; 45 | } 46 | } 47 | 48 | Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it) 49 | { 50 | //gets the frame for the current element (segment) 51 | const TreeElementType& currentElement = it->second; 52 | Frame currentFrame = GetTreeElementSegment(currentElement).pose(q_in(GetTreeElementQNr(currentElement))); 53 | 54 | SegmentMap::const_iterator rootIterator = tree.getRootSegment(); 55 | if(it == rootIterator){ 56 | return currentFrame; 57 | } 58 | else{ 59 | SegmentMap::const_iterator parentIt = GetTreeElementParent(currentElement); 60 | return recursiveFk(q_in, parentIt) * currentFrame; 61 | } 62 | } 63 | 64 | TreeFkSolverPos_recursive::~TreeFkSolverPos_recursive() 65 | { 66 | } 67 | 68 | 69 | } 70 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/treefksolverpos_recursive.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | // Copyright (C) 2008 Julia Jesse 3 | 4 | // Version: 1.0 5 | // Author: Ruben Smits 6 | // Maintainer: Ruben Smits 7 | // URL: http://www.orocos.org/kdl 8 | 9 | // This library is free software; you can redistribute it and/or 10 | // modify it under the terms of the GNU Lesser General Public 11 | // License as published by the Free Software Foundation; either 12 | // version 2.1 of the License, or (at your option) any later version. 13 | 14 | // This library is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 17 | // Lesser General Public License for more details. 18 | 19 | // You should have received a copy of the GNU Lesser General Public 20 | // License along with this library; if not, write to the Free Software 21 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 22 | 23 | #ifndef KDLTREEFKSOLVERPOS_RECURSIVE_HPP 24 | #define KDLTREEFKSOLVERPOS_RECURSIVE_HPP 25 | 26 | #include "treefksolver.hpp" 27 | 28 | namespace KDL { 29 | 30 | /** 31 | * Implementation of a recursive forward position kinematics 32 | * algorithm to calculate the position transformation from joint 33 | * space to Cartesian space of a general kinematic tree (KDL::Tree). 34 | * 35 | * @ingroup KinematicFamily 36 | */ 37 | class TreeFkSolverPos_recursive : public TreeFkSolverPos 38 | { 39 | public: 40 | TreeFkSolverPos_recursive(const Tree& tree); 41 | ~TreeFkSolverPos_recursive(); 42 | 43 | virtual int JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName); 44 | 45 | private: 46 | const Tree tree; 47 | 48 | Frame recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it); 49 | }; 50 | 51 | } 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/treeiksolver.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * treeiksolver.hpp 3 | * 4 | * Created on: Nov 28, 2008 5 | * Author: rubensmits 6 | */ 7 | 8 | #ifndef TREEIKSOLVER_HPP_ 9 | #define TREEIKSOLVER_HPP_ 10 | 11 | #include "tree.hpp" 12 | #include "jntarray.hpp" 13 | #include "frames.hpp" 14 | #include 15 | 16 | namespace KDL { 17 | 18 | typedef std::map Twists; 19 | typedef std::map Jacobians; 20 | typedef std::map Frames; 21 | 22 | /** 23 | * \brief This abstract class encapsulates the inverse 24 | * position solver for a KDL::Chain. 25 | * 26 | * @ingroup KinematicFamily 27 | */ 28 | class TreeIkSolverPos { 29 | public: 30 | /** 31 | * Calculate inverse position kinematics, from cartesian 32 | *coordinates to joint coordinates. 33 | * 34 | * @param q_init initial guess of the joint coordinates 35 | * @param p_in input cartesian coordinates 36 | * @param q_out output joint coordinates 37 | * 38 | * @return if < 0 something went wrong 39 | * otherwise (>=0) remaining (weighted) distance to target 40 | */ 41 | virtual double CartToJnt(const JntArray& q_init, const Frames& p_in,JntArray& q_out)=0; 42 | 43 | virtual ~TreeIkSolverPos() { 44 | } 45 | ; 46 | }; 47 | 48 | /** 49 | * \brief This abstract class encapsulates the inverse 50 | * velocity solver for a KDL::Tree. 51 | * 52 | * @ingroup KinematicFamily 53 | */ 54 | class TreeIkSolverVel { 55 | public: 56 | /** 57 | * Calculate inverse velocity kinematics, from joint positions 58 | *and cartesian velocities to joint velocities. 59 | * 60 | * @param q_in input joint positions 61 | * @param v_in input cartesian velocity 62 | * @param qdot_out output joint velocities 63 | * 64 | * @return if < 0 something went wrong 65 | * distance to goal otherwise (weighted norm of v_in) 66 | */ 67 | virtual double CartToJnt(const JntArray& q_in, const Twists& v_in, JntArray& qdot_out)=0; 68 | 69 | virtual ~TreeIkSolverVel() { 70 | } 71 | ; 72 | 73 | }; 74 | 75 | } 76 | 77 | #endif /* TREEIKSOLVER_HPP_ */ 78 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/treeiksolverpos_nr_jl.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007-2008 Ruben Smits 2 | // Copyright (C) 2008 Mikael Mayer 3 | // Copyright (C) 2008 Julia Jesse 4 | 5 | // Version: 1.0 6 | // Author: Ruben Smits 7 | // Maintainer: Ruben Smits 8 | // URL: http://www.orocos.org/kdl 9 | 10 | // This library is free software; you can redistribute it and/or 11 | // modify it under the terms of the GNU Lesser General Public 12 | // License as published by the Free Software Foundation; either 13 | // version 2.1 of the License, or (at your option) any later version. 14 | 15 | // This library is distributed in the hope that it will be useful, 16 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 18 | // Lesser General Public License for more details. 19 | 20 | // You should have received a copy of the GNU Lesser General Public 21 | // License along with this library; if not, write to the Free Software 22 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 23 | 24 | #include "treeiksolverpos_nr_jl.hpp" 25 | 26 | namespace KDL { 27 | TreeIkSolverPos_NR_JL::TreeIkSolverPos_NR_JL(const Tree& _tree, 28 | const std::vector& _endpoints, 29 | const JntArray& _q_min, const JntArray& _q_max, 30 | TreeFkSolverPos& _fksolver, TreeIkSolverVel& _iksolver, 31 | unsigned int _maxiter, double _eps) : 32 | tree(_tree), q_min(_q_min), q_max(_q_max), iksolver(_iksolver), 33 | fksolver(_fksolver), delta_q(tree.getNrOfJoints()), 34 | endpoints(_endpoints), maxiter(_maxiter), eps(_eps) 35 | { 36 | for (size_t i = 0; i < endpoints.size(); i++) { 37 | frames.insert(Frames::value_type(endpoints[i], Frame::Identity())); 38 | delta_twists.insert(Twists::value_type(endpoints[i], Twist::Zero())); 39 | } 40 | } 41 | 42 | double TreeIkSolverPos_NR_JL::CartToJnt(const JntArray& q_init, const Frames& p_in, JntArray& q_out) { 43 | q_out = q_init; 44 | 45 | //First check if all elements in p_in are available: 46 | for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it) 47 | if(frames.find(f_des_it->first)==frames.end()) 48 | return -2; 49 | 50 | unsigned int k=0; 51 | while(++k <= maxiter) { 52 | for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it){ 53 | //Get all iterators for this endpoint 54 | Frames::iterator f_it = frames.find(f_des_it->first); 55 | Twists::iterator delta_twist = delta_twists.find(f_des_it->first); 56 | 57 | fksolver.JntToCart(q_out, f_it->second, f_it->first); 58 | delta_twist->second = diff(f_it->second, f_des_it->second); 59 | } 60 | double res = iksolver.CartToJnt(q_out, delta_twists, delta_q); 61 | if (res < eps) return res; 62 | 63 | Add(q_out, delta_q, q_out); 64 | 65 | for (unsigned int j = 0; j < q_min.rows(); j++) { 66 | if (q_out(j) < q_min(j)) 67 | q_out( j) = q_min(j); 68 | else if (q_out(j) > q_max(j)) 69 | q_out( j) = q_max(j); 70 | } 71 | } 72 | if (k <= maxiter) 73 | return 0; 74 | else 75 | return -3; 76 | } 77 | 78 | 79 | TreeIkSolverPos_NR_JL::~TreeIkSolverPos_NR_JL() { 80 | 81 | } 82 | 83 | }//namespace 84 | 85 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/treeiksolverpos_nr_jl.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007-2008 Ruben Smits 2 | // Copyright (C) 2008 Mikael Mayer 3 | // Copyright (C) 2008 Julia Jesse 4 | 5 | // Version: 1.0 6 | // Author: Ruben Smits 7 | // Maintainer: Ruben Smits 8 | // URL: http://www.orocos.org/kdl 9 | 10 | // This library is free software; you can redistribute it and/or 11 | // modify it under the terms of the GNU Lesser General Public 12 | // License as published by the Free Software Foundation; either 13 | // version 2.1 of the License, or (at your option) any later version. 14 | 15 | // This library is distributed in the hope that it will be useful, 16 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 18 | // Lesser General Public License for more details. 19 | 20 | // You should have received a copy of the GNU Lesser General Public 21 | // License along with this library; if not, write to the Free Software 22 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 23 | 24 | #ifndef KDLTREEIKSOLVERPOS_NR_JL_HPP 25 | #define KDLTREEIKSOLVERPOS_NR_JL_HPP 26 | 27 | #include "treeiksolver.hpp" 28 | #include "treefksolver.hpp" 29 | #include 30 | #include 31 | 32 | namespace KDL { 33 | 34 | /** 35 | * Implementation of a general inverse position kinematics 36 | * algorithm based on Newton-Raphson iterations to calculate the 37 | * position transformation from Cartesian to joint space of a general 38 | * KDL::Tree. Takes joint limits into account. 39 | * 40 | * @ingroup KinematicFamily 41 | */ 42 | class TreeIkSolverPos_NR_JL: public TreeIkSolverPos { 43 | public: 44 | /** 45 | * Constructor of the solver, it needs the tree, a forward 46 | * position kinematics solver and an inverse velocity 47 | * kinematics solver for that tree, and a list of the segments you are interested in. 48 | * 49 | * @param tree the tree to calculate the inverse position for 50 | * @param endpoints the list of endpoints you are interested in. 51 | * @param q_max the maximum joint positions 52 | * @param q_min the minimum joint positions 53 | * @param fksolver a forward position kinematics solver 54 | * @param iksolver an inverse velocity kinematics solver 55 | * @param maxiter the maximum Newton-Raphson iterations, 56 | * default: 100 57 | * @param eps the precision for the position, used to end the 58 | * iterations, default: epsilon (defined in kdl.hpp) 59 | * 60 | * @return 61 | */ 62 | TreeIkSolverPos_NR_JL(const Tree& tree, const std::vector& endpoints, const JntArray& q_min, const JntArray& q_max, TreeFkSolverPos& fksolver,TreeIkSolverVel& iksolver,unsigned int maxiter=100,double eps=1e-6); 63 | ~TreeIkSolverPos_NR_JL(); 64 | 65 | virtual double CartToJnt(const JntArray& q_init, const Frames& p_in, JntArray& q_out); 66 | 67 | private: 68 | const Tree tree; 69 | JntArray q_min; 70 | JntArray q_max; 71 | TreeIkSolverVel& iksolver; 72 | TreeFkSolverPos& fksolver; 73 | JntArray delta_q; 74 | Frames frames; 75 | Twists delta_twists; 76 | std::vector endpoints; 77 | 78 | unsigned int maxiter; 79 | double eps; 80 | }; 81 | 82 | } 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/treejnttojacsolver.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TreeJntToJacSolver.cpp 3 | * 4 | * Created on: Nov 27, 2008 5 | * Author: rubensmits 6 | */ 7 | 8 | #include "treejnttojacsolver.hpp" 9 | #include 10 | #include "kinfam_io.hpp" 11 | 12 | namespace KDL { 13 | 14 | TreeJntToJacSolver::TreeJntToJacSolver(const Tree& tree_in) : 15 | tree(tree_in) { 16 | } 17 | 18 | TreeJntToJacSolver::~TreeJntToJacSolver() { 19 | } 20 | 21 | int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, const std::string& segmentname) { 22 | //First we check all the sizes: 23 | if (q_in.rows() != tree.getNrOfJoints() || jac.columns() != tree.getNrOfJoints()) 24 | return -1; 25 | 26 | //Lets search the tree-element 27 | SegmentMap::const_iterator it = tree.getSegments().find(segmentname); 28 | 29 | //If segmentname is not inside the tree, back out: 30 | if (it == tree.getSegments().end()) 31 | return -2; 32 | 33 | //Let's make the jacobian zero: 34 | SetToZero(jac); 35 | 36 | SegmentMap::const_iterator root = tree.getRootSegment(); 37 | 38 | Frame T_total = Frame::Identity(); 39 | //Lets recursively iterate until we are in the root segment 40 | while (it != root) { 41 | //get the corresponding q_nr for this TreeElement: 42 | unsigned int q_nr = GetTreeElementQNr(it->second); 43 | 44 | //get the pose of the segment: 45 | Frame T_local = GetTreeElementSegment(it->second).pose(q_in(q_nr)); 46 | //calculate new T_end: 47 | T_total = T_local * T_total; 48 | 49 | //get the twist of the segment: 50 | if (GetTreeElementSegment(it->second).getJoint().getType() != Joint::None) { 51 | Twist t_local = GetTreeElementSegment(it->second).twist(q_in(q_nr), 1.0); 52 | //transform the endpoint of the local twist to the global endpoint: 53 | t_local = t_local.RefPoint(T_total.p - T_local.p); 54 | //transform the base of the twist to the endpoint 55 | t_local = T_total.M.Inverse(t_local); 56 | //store the twist in the jacobian: 57 | jac.setColumn(q_nr,t_local); 58 | }//endif 59 | //goto the parent 60 | it = GetTreeElementParent(it->second); 61 | }//endwhile 62 | //Change the base of the complete jacobian from the endpoint to the base 63 | changeBase(jac, T_total.M, jac); 64 | 65 | return 0; 66 | 67 | }//end JntToJac 68 | }//end namespace 69 | 70 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/treejnttojacsolver.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * TreeJntToJacSolver.hpp 3 | * 4 | * Created on: Nov 27, 2008 5 | * Author: rubensmits 6 | */ 7 | 8 | #ifndef TREEJNTTOJACSOLVER_HPP_ 9 | #define TREEJNTTOJACSOLVER_HPP_ 10 | 11 | #include "tree.hpp" 12 | #include "jacobian.hpp" 13 | #include "jntarray.hpp" 14 | 15 | namespace KDL { 16 | 17 | class TreeJntToJacSolver { 18 | public: 19 | explicit TreeJntToJacSolver(const Tree& tree); 20 | 21 | virtual ~TreeJntToJacSolver(); 22 | 23 | /* 24 | * Calculate the jacobian for a part of the tree: from a certain segment, given by segmentname to the root. 25 | * The resulting jacobian is expressed in the baseframe of the tree ("root"), the reference point is in the end-segment 26 | */ 27 | 28 | int JntToJac(const JntArray& q_in, Jacobian& jac, 29 | const std::string& segmentname); 30 | 31 | private: 32 | KDL::Tree tree; 33 | 34 | }; 35 | 36 | }//End of namespace 37 | 38 | #endif /* TREEJNTTOJACSOLVER_H_ */ 39 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/utilities/error_stack.cxx: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * \author 3 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 4 | * 5 | * \version 6 | * ORO_Geometry V0.2 7 | * 8 | * \par History 9 | * - $log$ 10 | * 11 | * \par Release 12 | * $Id: error_stack.cpp,v 1.1.1.1.2.1 2003/02/24 13:13:06 psoetens Exp $ 13 | * $Name: $ 14 | ****************************************************************************/ 15 | 16 | 17 | #include "error_stack.h" 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace KDL { 24 | 25 | // Trace of the call stack of the I/O routines to help user 26 | // interpret error messages from I/O 27 | typedef std::stack ErrorStack; 28 | 29 | // should be in Thread Local Storage if this gets multithreaded one day... 30 | static ErrorStack errorstack; 31 | 32 | 33 | void IOTrace(const std::string& description) { 34 | errorstack.push(description); 35 | } 36 | 37 | 38 | void IOTracePop() { 39 | errorstack.pop(); 40 | } 41 | 42 | void IOTraceOutput(std::ostream& os) { 43 | while (!errorstack.empty()) { 44 | os << errorstack.top().c_str() << std::endl; 45 | errorstack.pop(); 46 | } 47 | } 48 | 49 | 50 | void IOTracePopStr(char* buffer,int size) { 51 | if (size <= 0) 52 | { 53 | // TODO: all sizes everywhere should be of size_t! 54 | return; 55 | } 56 | if (errorstack.empty()) { 57 | *buffer = 0; 58 | return; 59 | } 60 | strncpy(buffer,errorstack.top().c_str(),size); 61 | buffer[size - 1] = '\0'; 62 | errorstack.pop(); 63 | } 64 | 65 | } 66 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/utilities/error_stack.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Erwin Aertbelien Mon Jan 10 16:38:39 CET 2005 error_stack.h 3 | 4 | error_stack.h - description 5 | ------------------- 6 | begin : Mon January 10 2005 7 | copyright : (C) 2005 Erwin Aertbelien 8 | email : erwin.aertbelien@mech.kuleuven.ac.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | 29 | /** 30 | * \file 31 | * \author Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 32 | * \version 33 | * ORO_Geometry V0.2 34 | * 35 | * \par history 36 | * - changed layout of the comments to accommodate doxygen 37 | */ 38 | #ifndef ERROR_STACK_H 39 | #define ERROR_STACK_H 40 | 41 | #include "utility.h" 42 | #include "utility_io.h" 43 | #include 44 | 45 | 46 | namespace KDL { 47 | 48 | /* 49 | * \todo 50 | * IOTrace-routines store in static memory, should be in thread-local memory. 51 | * pushes a description of the current routine on the IO-stack trace 52 | */ 53 | void IOTrace(const std::string& description); 54 | 55 | //! pops a description of the IO-stack 56 | void IOTracePop(); 57 | 58 | 59 | //! outputs the IO-stack to a stream to provide a better errormessage. 60 | void IOTraceOutput(std::ostream& os); 61 | 62 | //! outputs one element of the IO-stack to the buffer (maximally size chars) 63 | //! returns empty string if no elements on the stack. 64 | void IOTracePopStr(char* buffer,int size); 65 | 66 | 67 | } 68 | 69 | #endif 70 | 71 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/utilities/header.txt: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | XXXXXX - description 3 | ------------------------- 4 | begin : June 2006 5 | copyright : (C) 2006 Erwin Aertbelien 6 | email : firstname.lastname@mech.kuleuven.ac.be 7 | 8 | History (only major changes)( AUTHOR-Description ) : 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/utilities/kdl-config.h: -------------------------------------------------------------------------------- 1 | /* Copyright (C) 2007 Ruben Smits */ 2 | 3 | /* Version: 1.0 */ 4 | /* Author: Ruben Smits */ 5 | /* Maintainer: Ruben Smits */ 6 | /* URL: http://www.orocos.org/kdl */ 7 | 8 | /* This library is free software; you can redistribute it and/or */ 9 | /* modify it under the terms of the GNU Lesser General Public */ 10 | /* License as published by the Free Software Foundation; either */ 11 | /* version 2.1 of the License, or (at your option) any later version. */ 12 | 13 | /* This library is distributed in the hope that it will be useful, */ 14 | /* but WITHOUT ANY WARRANTY; without even the implied warranty of */ 15 | /* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU */ 16 | /* Lesser General Public License for more details. */ 17 | 18 | /* You should have received a copy of the GNU Lesser General Public */ 19 | /* License along with this library; if not, write to the Free Software */ 20 | /* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA */ 21 | 22 | 23 | /* Methods are inlined */ 24 | #define KDL_INLINE 1 25 | 26 | /* Column width that is used form printing frames */ 27 | #define KDL_FRAME_WIDTH 12 28 | 29 | /* Indices are checked when accessing members of the objects */ 30 | #define KDL_INDEX_CHECK 1 31 | 32 | /* use KDL implementation for == operator */ 33 | #define KDL_USE_EQUAL 1 34 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/utilities/rall1d_io.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * \file 3 | * provides I/O operations on Rall1d 4 | * 5 | * \author 6 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 7 | * 8 | * \version 9 | * ORO_Geometry V0.2 10 | * 11 | * \par History 12 | * - $log$ 13 | * 14 | * \par Release 15 | * $Id: rall1d_io.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ 16 | * $Name: $ 17 | ****************************************************************************/ 18 | #ifndef Rall_IO_H 19 | #define Rall_IO_H 20 | 21 | #include "utility_io.h" 22 | #include "rall1d.h" 23 | 24 | namespace KDL { 25 | 26 | template 27 | inline std::ostream& operator << (std::ostream& os,const Rall1d& r) 28 | { 29 | os << "Rall1d(" << r.t <<"," << r.grad <<")"; 30 | return os; 31 | } 32 | 33 | 34 | } 35 | 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/utilities/rall2d_io.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * \file 3 | * provides I/O operations on Rall1d 4 | * 5 | * \author 6 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 7 | * 8 | * \version 9 | * ORO_Geometry V0.2 10 | * 11 | * \par History 12 | * - $log$ 13 | * 14 | * \par Release 15 | * $Id: rall2d_io.h,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $ 16 | * $Name: $ 17 | ****************************************************************************/ 18 | #ifndef Rall2d_IO_H 19 | #define Rall2d_IO_H 20 | 21 | 22 | 23 | #include "utility_io.h" 24 | #include "rall2d.h" 25 | 26 | namespace KDL { 27 | 28 | template 29 | std::ostream& operator << (std::ostream& os,const Rall2d& r) 30 | { 31 | os << "Rall2d(" << r.t <<"," << r.d <<","< 15 | class RallNd : 16 | public Rall1d< RallNd, RallNd, double > 17 | { 18 | public: 19 | RallNd() {} 20 | RallNd(const Rall1d< RallNd, RallNd,double>& arg) : 21 | Rall1d< RallNd, RallNd,double>(arg) {} 22 | RallNd(double value,double d[]) { 23 | this->t = RallNd(value,d); 24 | this->grad = RallNd(d[0],&d[1]); 25 | } 26 | }; 27 | 28 | template <> 29 | class RallNd<1> : public Rall1d { 30 | public: 31 | RallNd() {} 32 | RallNd(const Rall1d& arg) : 33 | Rall1d(arg) {} 34 | RallNd(double value,double d[]) { 35 | t = value; 36 | grad = d[0]; 37 | } 38 | }; 39 | */ 40 | /** 41 | * to be checked.. 42 | */ 43 | 44 | /** 45 | * Als je tot 3de orde een efficiente berekening kan doen, 46 | * dan kan je tot een willekeurige orde alles efficient berekenen 47 | * 0 1 2 3 48 | * ==> 1 2 3 4 49 | * ==> 3 4 5 6 50 | * 4 5 6 7 51 | * 52 | * de aangeduide berekeningen zijn niet noodzakelijk, en er is dan niets 53 | * verniet berekend in de recursieve implementatie. 54 | * of met 2de orde over 1ste order : kan ook efficient : 55 | * 0 1 56 | * ==>1 2 57 | * 2 3 58 | */ 59 | // N>2: 60 | template 61 | class RallNd : 62 | public Rall2d< RallNd, RallNd, double > 63 | { 64 | public: 65 | RallNd() {} 66 | RallNd(const Rall2d< RallNd, RallNd,double>& arg) : 67 | Rall2d< RallNd, RallNd,double>(arg) {} 68 | RallNd(double value,double d[]) { 69 | this->t = RallNd(value,d); // 0 1 2 70 | this->d = RallNd(d[0],&d[1]); // 1 2 3 iseigenlijk niet nodig 71 | this->dd = RallNd(d[1],&d[2]); // 2 3 4 72 | } 73 | }; 74 | 75 | template <> 76 | class RallNd<2> : public Rall2d { 77 | public: 78 | RallNd() {}* (dwz. met evenveel numerieke operaties als een 79 | RallNd(const Rall2d& arg) : 80 | Rall2d(arg) {} 81 | RallNd(double value,double d[]) { 82 | t = value; 83 | d = d[0]; 84 | dd= d[1]; 85 | } 86 | }; 87 | 88 | template <> 89 | class RallNd<1> : public Rall1d { 90 | public: 91 | RallNd() {} 92 | RallNd(const Rall1d& arg) : 93 | Rall1d(arg) {} 94 | RallNd(double value,double d[]) { 95 | t = value; 96 | grad = d[0]; 97 | } 98 | }; 99 | 100 | #endif 101 | 102 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/utilities/svd_HH.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | #ifndef KDL_SVD_HH_HPP 23 | #define KDL_SVD_HH_HPP 24 | 25 | #include "../jacobian.hpp" 26 | #include "../jntarray.hpp" 27 | #include 28 | 29 | namespace KDL 30 | { 31 | 32 | class SVD_HH 33 | { 34 | public: 35 | SVD_HH(const Jacobian& jac); 36 | ~SVD_HH(); 37 | 38 | int calculate(const Jacobian& jac,std::vector& U, 39 | JntArray& w,std::vector& v,int maxiter); 40 | private: 41 | JntArray tmp; 42 | }; 43 | } 44 | #endif 45 | 46 | 47 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/utilities/svd_eigen_HH.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2007 Ruben Smits 2 | 3 | // Version: 1.0 4 | // Author: Ruben Smits 5 | // Maintainer: Ruben Smits 6 | // URL: http://www.orocos.org/kdl 7 | 8 | // This library is free software; you can redistribute it and/or 9 | // modify it under the terms of the GNU Lesser General Public 10 | // License as published by the Free Software Foundation; either 11 | // version 2.1 of the License, or (at your option) any later version. 12 | 13 | // This library is distributed in the hope that it will be useful, 14 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 16 | // Lesser General Public License for more details. 17 | 18 | // You should have received a copy of the GNU Lesser General Public 19 | // License along with this library; if not, write to the Free Software 20 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | 22 | 23 | //Based on the svd of the KDL-0.2 library by Erwin Aertbelien 24 | #ifndef SVD_EIGEN_HH_HPP 25 | #define SVD_EIGEN_HH_HPP 26 | 27 | 28 | #include 29 | #include 30 | 31 | using namespace Eigen; 32 | 33 | namespace KDL 34 | { 35 | inline double PYTHAG(double a,double b) { 36 | double at,bt,ct; 37 | at = fabs(a); 38 | bt = fabs(b); 39 | if (at > bt ) { 40 | ct=bt/at; 41 | return at*sqrt(1.0+ct*ct); 42 | } else { 43 | if (bt==0) 44 | return 0.0; 45 | else { 46 | ct=at/bt; 47 | return bt*sqrt(1.0+ct*ct); 48 | } 49 | } 50 | } 51 | 52 | 53 | inline double SIGN(double a,double b) { 54 | return ((b) >= 0.0 ? fabs(a) : -fabs(a)); 55 | } 56 | 57 | /** 58 | * svd calculation of eigen matrices 59 | * 60 | * @param A matrix(mxn) 61 | * @param U matrix(mxn) 62 | * @param S vector n 63 | * @param V matrix(nxn) 64 | * @param tmp vector n 65 | * @param maxiter defaults to 150 66 | * 67 | * @return -2 if maxiter exceeded, 0 otherwise 68 | */ 69 | int svd_eigen_HH(const MatrixXd& A,MatrixXd& U,VectorXd& S,MatrixXd& V,VectorXd& tmp,int maxiter=150,double epsilon=1e-300); 70 | } 71 | #endif 72 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/utilities/traits.h: -------------------------------------------------------------------------------- 1 | #ifndef KDLPV_TRAITS_H 2 | #define KDLPV_TRAITS_H 3 | 4 | #include "utility.h" 5 | 6 | 7 | // forwards declarations : 8 | namespace KDL { 9 | class Frame; 10 | class Rotation; 11 | class Vector; 12 | class Twist; 13 | class Wrench; 14 | class FrameVel; 15 | class RotationVel; 16 | class VectorVel; 17 | class TwistVel; 18 | } 19 | 20 | 21 | /** 22 | * @brief Traits are traits classes to determine the type of a derivative of another type. 23 | * 24 | * For geometric objects the "geometric" derivative is chosen. For example the derivative of a Rotation 25 | * matrix is NOT a 3x3 matrix containing the derivative of the elements of a rotation matrix. The derivative 26 | * of the rotation matrix is a Vector corresponding the rotational velocity. Mostly used in template classes 27 | * and routines to derive a correct type when needed. 28 | * 29 | * You can see this as a compile-time lookuptable to find the type of the derivative. 30 | * 31 | * Example 32 | * \verbatim 33 | Rotation R; 34 | Traits dR; 35 | \endverbatim 36 | */ 37 | template 38 | struct Traits { 39 | typedef T valueType; 40 | typedef T derivType; 41 | }; 42 | 43 | template <> 44 | struct Traits { 45 | typedef KDL::Frame valueType; 46 | typedef KDL::Twist derivType; 47 | }; 48 | template <> 49 | struct Traits { 50 | typedef KDL::Twist valueType; 51 | typedef KDL::Twist derivType; 52 | }; 53 | template <> 54 | struct Traits { 55 | typedef KDL::Wrench valueType; 56 | typedef KDL::Wrench derivType; 57 | }; 58 | 59 | template <> 60 | struct Traits { 61 | typedef KDL::Rotation valueType; 62 | typedef KDL::Vector derivType; 63 | }; 64 | 65 | template <> 66 | struct Traits { 67 | typedef KDL::Vector valueType; 68 | typedef KDL::Vector derivType; 69 | }; 70 | 71 | template <> 72 | struct Traits { 73 | typedef double valueType; 74 | typedef double derivType; 75 | }; 76 | 77 | template <> 78 | struct Traits { 79 | typedef float valueType; 80 | typedef float derivType; 81 | }; 82 | 83 | template <> 84 | struct Traits { 85 | typedef KDL::Frame valueType; 86 | typedef KDL::TwistVel derivType; 87 | }; 88 | template <> 89 | struct Traits { 90 | typedef KDL::Twist valueType; 91 | typedef KDL::TwistVel derivType; 92 | }; 93 | 94 | template <> 95 | struct Traits { 96 | typedef KDL::Rotation valueType; 97 | typedef KDL::VectorVel derivType; 98 | }; 99 | 100 | template <> 101 | struct Traits { 102 | typedef KDL::Vector valueType; 103 | typedef KDL::VectorVel derivType; 104 | }; 105 | 106 | 107 | 108 | #endif 109 | 110 | 111 | 112 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/utilities/utility.cxx: -------------------------------------------------------------------------------- 1 | /** @file utility.cpp 2 | * @author Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 3 | * @version 4 | * ORO_Geometry V0.2 5 | * 6 | * @par history 7 | * - changed layout of the comments to accommodate doxygen 8 | */ 9 | 10 | #include "utility.h" 11 | 12 | namespace KDL { 13 | 14 | int STREAMBUFFERSIZE=10000; 15 | int MAXLENFILENAME = 255; 16 | const double PI= 3.1415926535897932384626433832795; 17 | const double deg2rad = 0.01745329251994329576923690768488; 18 | const double rad2deg = 57.2957795130823208767981548141052; 19 | double epsilon = 0.000001; 20 | } 21 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/utilities/utility_io.h: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * \author 3 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 4 | * 5 | * \version 6 | * ORO_Geometry V0.2 7 | * 8 | * \par History 9 | * - $log$ 10 | * 11 | * \par Release 12 | * $Id: utility_io.h,v 1.1.1.1.2.3 2003/06/26 15:23:59 psoetens Exp $ 13 | * $Name: $ 14 | * 15 | * \file utility_io.h 16 | * Included by most lrl-files to provide some general 17 | * functions and macro definitions related to file/stream I/O. 18 | */ 19 | 20 | #ifndef KDL_UTILITY_IO_H_84822 21 | #define KDL_UTILITY_IO_H_84822 22 | 23 | //#include 24 | 25 | 26 | // Standard includes 27 | #include 28 | #include 29 | #include 30 | 31 | 32 | namespace KDL { 33 | 34 | 35 | /** 36 | * checks validity of basic io of is 37 | */ 38 | void _check_istream(std::istream& is); 39 | 40 | 41 | /** 42 | * Eats characters of the stream until the character delim is encountered 43 | * @param is a stream 44 | * @param delim eat until this character is encountered 45 | */ 46 | void Eat(std::istream& is, int delim ); 47 | 48 | /** 49 | * Eats characters of the stream as long as they satisfy the description in descript 50 | * @param is a stream 51 | * @param descript description string. A sequence of spaces, tabs, 52 | * new-lines and comments is regarded as 1 space in the description string. 53 | */ 54 | void Eat(std::istream& is,const char* descript); 55 | 56 | /** 57 | * Eats a word of the stream delimited by the letters in delim or space(tabs...) 58 | * @param is a stream 59 | * @param delim a string containing the delimmiting characters 60 | * @param storage for returning the word 61 | * @param maxsize a word can be maximally maxsize-1 long. 62 | */ 63 | void EatWord(std::istream& is,const char* delim,char* storage,int maxsize); 64 | 65 | /** 66 | * Eats characters of the stream until the character delim is encountered 67 | * similar to Eat(is,delim) but spaces at the end are not read. 68 | * @param is a stream 69 | * @param delim eat until this character is encountered 70 | */ 71 | void EatEnd( std::istream& is, int delim ); 72 | 73 | 74 | 75 | 76 | } 77 | 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/velocityprofile.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Erwin Aertbelien Mon May 10 19:10:36 CEST 2004 velocityprofile.cxx 3 | 4 | velocityprofile.cxx - description 5 | ------------------- 6 | begin : Mon May 10 2004 7 | copyright : (C) 2004 Erwin Aertbelien 8 | email : erwin.aertbelien@mech.kuleuven.ac.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | /***************************************************************************** 28 | * \author 29 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 30 | * 31 | * \version 32 | * ORO_Geometry V0.2 33 | * 34 | * \par History 35 | * - $log$ 36 | * 37 | * \par Release 38 | * $Id: velocityprofile.cpp,v 1.1.1.1.2.3 2003/02/24 13:13:06 psoetens Exp $ 39 | * $Name: $ 40 | ****************************************************************************/ 41 | 42 | 43 | #include "utilities/error.h" 44 | #include "utilities/error_stack.h" 45 | #include "velocityprofile_rect.hpp" 46 | #include "velocityprofile_dirac.hpp" 47 | #include "velocityprofile_trap.hpp" 48 | #include "velocityprofile_traphalf.hpp" 49 | #include 50 | 51 | namespace KDL { 52 | 53 | using namespace std; 54 | 55 | VelocityProfile* VelocityProfile::Read(istream& is) { 56 | IOTrace("VelocityProfile::Read"); 57 | char storage[25]; 58 | EatWord(is,"[",storage,sizeof(storage)); 59 | Eat(is,'['); 60 | if (strcmp(storage,"DIRACVEL")==0) { 61 | Eat(is,']'); 62 | IOTracePop(); 63 | return new VelocityProfile_Dirac(); 64 | } else if (strcmp(storage,"CONSTVEL")==0) { 65 | double vel; 66 | is >> vel; 67 | Eat(is,']'); 68 | IOTracePop(); 69 | return new VelocityProfile_Rectangular(vel); 70 | } else if (strcmp(storage,"TRAPEZOIDAL")==0) { 71 | double maxvel; 72 | double maxacc; 73 | is >> maxvel; 74 | Eat(is,','); 75 | is >> maxacc; 76 | Eat(is,']'); 77 | IOTracePop(); 78 | return new VelocityProfile_Trap(maxvel,maxacc); 79 | } else if (strcmp(storage,"TRAPEZOIDALHALF")==0) { 80 | double maxvel; 81 | double maxacc; 82 | is >> maxvel; 83 | Eat(is,','); 84 | is >> maxacc; 85 | Eat(is,','); 86 | bool starting; 87 | is >> starting; 88 | Eat(is,']'); 89 | IOTracePop(); 90 | return new VelocityProfile_TrapHalf(maxvel,maxacc,starting); 91 | } 92 | else { 93 | throw Error_MotionIO_Unexpected_MotProf(); 94 | } 95 | return 0; 96 | } 97 | 98 | 99 | 100 | } 101 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/velocityprofile_dirac.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Peter Soetens Mon May 10 19:10:36 CEST 2004 velocityprofile_dirac.cxx 3 | 4 | velocityprofile_dirac.cxx - description 5 | ------------------- 6 | begin : Mon May 10 2004 7 | copyright : (C) 2004 Peter Soetens 8 | email : peter.soetens@mech.kuleuven.ac.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #include "utilities/error.h" 29 | #include "velocityprofile_dirac.hpp" 30 | 31 | namespace KDL { 32 | 33 | 34 | void VelocityProfile_Dirac::SetProfile( 35 | double pos1, 36 | double pos2 37 | ) 38 | { 39 | p1 = pos1; 40 | p2 = pos2; 41 | t = 0; 42 | } 43 | 44 | void VelocityProfile_Dirac:: 45 | SetProfileDuration(double pos1,double pos2,double duration) 46 | { 47 | SetProfile(pos1,pos2); 48 | t = duration; 49 | } 50 | 51 | double VelocityProfile_Dirac::Duration() const { 52 | return t; 53 | } 54 | 55 | double VelocityProfile_Dirac::Pos(double time) const { 56 | if ( t == 0 ) { 57 | return time <= 0 ? p1 : p2; 58 | } else if (time < 0) { 59 | return p1; 60 | } else if (time <= t) { 61 | return p1 + (( p2 - p1)/t)*time; 62 | } else { 63 | return p2; 64 | } 65 | } 66 | 67 | double VelocityProfile_Dirac::Vel(double time) const { 68 | if ( t == 0 ) 69 | { 70 | throw Error_MotionPlanning_Incompatible(); 71 | } 72 | else 73 | if ( 0 < time && time < t ) 74 | return (p2-p1) / t; 75 | return 0; 76 | } 77 | 78 | double VelocityProfile_Dirac::Acc(double time) const { 79 | throw Error_MotionPlanning_Incompatible(); 80 | return 0; 81 | } 82 | 83 | 84 | void VelocityProfile_Dirac::Write(std::ostream& os) const { 85 | os << "DIRACVEL[ ]"; 86 | } 87 | 88 | 89 | 90 | } 91 | 92 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/velocityprofile_dirac.hpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Peter Soetens Fri Feb 11 15:59:12 CET 2005 velocityprofile_dirac.h 3 | 4 | velocityprofile_dirac.h - description 5 | ------------------- 6 | begin : Fri February 11 2005 7 | copyright : (C) 2005 Peter Soetens 8 | email : peter.soetens@mech.kuleuven.ac.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | 29 | #ifndef MOTIONPROFILE_DIRAC_H 30 | #define MOTIONPROFILE_DIRAC_H 31 | 32 | #include "velocityprofile.hpp" 33 | 34 | 35 | namespace KDL { 36 | /** 37 | * A Dirac VelocityProfile generates an infinite velocity 38 | * so that the position jumps from A to B in in infinite short time. 39 | * In practice, this means that the maximum values are ignored and 40 | * for any t : Vel(t) == 0 and Acc(t) == 0. 41 | * Further Pos( -0 ) = pos1 and Pos( +0 ) = pos2. 42 | * 43 | * However, if a duration is given, it will create an unbound 44 | * rectangular velocity profile for that duration, otherwise, 45 | * Duration() == 0; 46 | * @ingroup Motion 47 | */ 48 | class VelocityProfile_Dirac : public VelocityProfile 49 | { 50 | double p1,p2,t; 51 | public: 52 | void SetProfile(double pos1,double pos2); 53 | virtual void SetProfileDuration(double pos1,double pos2,double duration); 54 | virtual double Duration() const; 55 | virtual double Pos(double time) const; 56 | virtual double Vel(double time) const; 57 | virtual double Acc(double time) const; 58 | virtual void Write(std::ostream& os) const; 59 | virtual VelocityProfile* Clone() const { 60 | VelocityProfile_Dirac* res = new VelocityProfile_Dirac(); 61 | res->SetProfileDuration( p1, p2, t ); 62 | return res; 63 | } 64 | 65 | virtual ~VelocityProfile_Dirac() {} 66 | }; 67 | 68 | } 69 | 70 | 71 | #endif 72 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/velocityprofile_rect.hpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Erwin Aertbelien Mon Jan 10 16:38:38 CET 2005 velocityprofile_rect.h 3 | 4 | velocityprofile_rect.h - description 5 | ------------------- 6 | begin : Mon January 10 2005 7 | copyright : (C) 2005 Erwin Aertbelien 8 | email : erwin.aertbelien@mech.kuleuven.ac.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | 29 | /***************************************************************************** 30 | * \author 31 | * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven 32 | * 33 | * \version 34 | * ORO_Geometry V0.2 35 | * 36 | * \par History 37 | * - $log$ 38 | * 39 | * \par Release 40 | * $Id: velocityprofile_rect.h,v 1.1.1.1.2.4 2003/07/24 13:26:15 psoetens Exp $ 41 | * $Name: $ 42 | ****************************************************************************/ 43 | 44 | #ifndef MOTIONPROFILE_RECT_H 45 | #define MOTIONPROFILE_RECT_H 46 | 47 | #include "velocityprofile.hpp" 48 | 49 | 50 | namespace KDL { 51 | /** 52 | * A rectangular VelocityProfile generates a constant velocity 53 | * for moving from A to B. 54 | * @ingroup Motion 55 | */ 56 | class VelocityProfile_Rectangular : public VelocityProfile 57 | // Defines a rectangular velocityprofile. 58 | // (i.e. constant velocity) 59 | { 60 | double d,p,v; 61 | public: 62 | double maxvel; 63 | 64 | VelocityProfile_Rectangular(double _maxvel=0): 65 | maxvel(_maxvel) {} 66 | // constructs motion profile class with as parameter of the 67 | // trajectory. 68 | 69 | void SetMax( double _maxvel ); 70 | void SetProfile(double pos1,double pos2); 71 | virtual void SetProfileDuration( 72 | double pos1,double pos2,double duration); 73 | virtual double Duration() const; 74 | virtual double Pos(double time) const; 75 | virtual double Vel(double time) const; 76 | virtual double Acc(double time) const; 77 | virtual void Write(std::ostream& os) const; 78 | virtual VelocityProfile* Clone() const{ 79 | VelocityProfile_Rectangular* res = new VelocityProfile_Rectangular(maxvel); 80 | res->SetProfileDuration( p, p+v*d, d ); 81 | return res; 82 | } 83 | // returns copy of current VelocityProfile object. (virtual constructor) 84 | virtual ~VelocityProfile_Rectangular() {} 85 | }; 86 | 87 | } 88 | 89 | 90 | #endif 91 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl/velocityprofile_spline.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VELOCITYPROFILE_SPLINE_H 2 | #define VELOCITYPROFILE_SPLINE_H 3 | 4 | #include "velocityprofile.hpp" 5 | 6 | namespace KDL 7 | { 8 | /** 9 | * \brief A spline VelocityProfile trajectory interpolation. 10 | * @ingroup Motion 11 | */ 12 | class VelocityProfile_Spline : public VelocityProfile 13 | { 14 | public: 15 | VelocityProfile_Spline(); 16 | VelocityProfile_Spline(const VelocityProfile_Spline &p); 17 | 18 | virtual ~VelocityProfile_Spline(); 19 | 20 | virtual void SetProfile(double pos1, double pos2); 21 | /** 22 | * Generate linear interpolation coefficients. 23 | * 24 | * @param pos1 begin position. 25 | * @param pos2 end position. 26 | * @param duration duration of the profile. 27 | */ 28 | virtual void SetProfileDuration( 29 | double pos1, double pos2, double duration); 30 | 31 | /** 32 | * Generate cubic spline interpolation coefficients. 33 | * 34 | * @param pos1 begin position. 35 | * @param vel1 begin velocity. 36 | * @param pos2 end position. 37 | * @param vel2 end velocity. 38 | * @param duration duration of the profile. 39 | */ 40 | virtual void SetProfileDuration( 41 | double pos1, double vel1, double pos2, double vel2, double duration); 42 | 43 | /** 44 | * Generate quintic spline interpolation coefficients. 45 | * 46 | * @param pos1 begin position. 47 | * @param vel1 begin velocity. 48 | * @param acc1 begin acceleration 49 | * @param pos2 end position. 50 | * @param vel2 end velocity. 51 | * @param acc2 end acceleration. 52 | * @param duration duration of the profile. 53 | */ 54 | virtual void SetProfileDuration(double pos1, double vel1, double acc1, double pos2, double vel2, double acc2, double duration); 55 | virtual double Duration() const; 56 | virtual double Pos(double time) const; 57 | virtual double Vel(double time) const; 58 | virtual double Acc(double time) const; 59 | virtual void Write(std::ostream& os) const; 60 | virtual VelocityProfile* Clone() const; 61 | private: 62 | 63 | double coeff_[6]; 64 | double duration_; 65 | }; 66 | } 67 | #endif // VELOCITYPROFILE_CUBICSPLINE_H 68 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl_ros_control/alglib/stdafx.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl_ros_control/kdl_control.h: -------------------------------------------------------------------------------- 1 | #ifndef KDLControl 2 | #define KDLControl 3 | 4 | #include "Eigen/Dense" 5 | #include "kdl_robot.h" 6 | #include "kdl_object.h" 7 | #include "utils.h" 8 | 9 | class KDLController 10 | { 11 | 12 | public: 13 | 14 | KDLController(KDLRobot &_robot, KDLObject &_obj); 15 | 16 | Eigen::VectorXd idCntr(KDL::JntArray &_qd, 17 | KDL::JntArray &_dqd, 18 | KDL::JntArray &_ddqd, 19 | double _Kp, 20 | double _Kd); 21 | 22 | Eigen::VectorXd idCntr(KDL::Frame &_desPos, 23 | KDL::Twist &_desVel, 24 | KDL::Twist &_desAcc, 25 | double _Kpp, 26 | double _Kpo, 27 | double _Kdp, 28 | double _Kdo); 29 | 30 | Eigen::VectorXd sharedCntr(KDL::Frame &_desObjPos, 31 | KDL::Twist &_desObjVel, 32 | KDL::Twist &_desObjAcc, 33 | double Kp, 34 | double Kd, 35 | bool _useTilting); 36 | 37 | private: 38 | 39 | Eigen::VectorXd tiltingTorque(double &cost, const double &lambda, const double &K); 40 | Eigen::VectorXd gradientFcAngles(const Eigen::VectorXd &a, const double &lambda, double &costValue); 41 | Eigen::VectorXd computeFcAngles(); 42 | KDLRobot* robot_; 43 | KDLObject* obj_; 44 | 45 | }; 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl_ros_control/kdl_object.h: -------------------------------------------------------------------------------- 1 | #ifndef KDLOBJECT 2 | #define KDLOBJECT 3 | 4 | #include "Eigen/Dense" 5 | #include "kdl/frames.hpp" 6 | #include "kdl/jacobian.hpp" 7 | #include "kdl_ros_control/alglib/optimization.h" 8 | 9 | class Contact 10 | { 11 | 12 | public: 13 | 14 | Contact(); // pass values to constructor 15 | 16 | KDL::Frame getFrame() const; 17 | KDL::Wrench getWrench() const; 18 | Eigen::Matrix getConeVersors() const; 19 | Eigen::Matrix getB() const; 20 | Eigen::Matrix getBo() const; 21 | double getTheta() const; 22 | 23 | void setFrame(const KDL::Frame &value); 24 | void setMu(const double _mu); 25 | void setWrench(const KDL::Wrench &value); 26 | void setTwist(const KDL::Twist &value); 27 | 28 | private: 29 | 30 | KDL::Frame F_; 31 | KDL::Twist T_; 32 | KDL::Wrench W_; 33 | Eigen::Matrix B_; 34 | Eigen::Matrix Bo_; 35 | Eigen::Matrix Fc_hat_; 36 | Eigen::Vector3d z_hat_; 37 | double mu_, theta_; 38 | }; 39 | 40 | class KDLObject 41 | { 42 | 43 | public: 44 | 45 | KDLObject(); 46 | KDLObject(double &m, Eigen::Matrix3d &I, std::vector &c, double &mu); 47 | 48 | double getMass() const; 49 | KDL::Frame getFrame() const; 50 | KDL::Wrench getBodyWrench() const; 51 | KDL::Twist getBodyVelocity() const; 52 | KDL::Twist getSpatialVelocity() const; 53 | std::vector getContacts() const; 54 | std::vector getContactsWrenches() const; 55 | Eigen::Matrix getGraspMatrix() const; 56 | Eigen::Matrix getGraspMatrixOri() const; 57 | Eigen::Matrix getInertia() const; 58 | Eigen::Matrix getMassMatrix() const; 59 | 60 | void setFrame(const KDL::Frame &value); 61 | void setBodyWrench(const KDL::Wrench &value); 62 | void setBodyVelocity(const KDL::Twist &value); 63 | void setSpatialVelocity(const KDL::Twist &value); 64 | void setContactFrames(const std::vector &value); 65 | void setContactWrenches(const std::vector &value); 66 | void setContactTwists(const std::vector &value); 67 | 68 | void addContact(const KDL::Frame &F_bc, const double _mu); 69 | void computeContactForces(KDL::Wrench &F_b); 70 | void computeContactVelocities(KDL::Twist &V_b); 71 | void computeC(); 72 | void computeN(); 73 | KDL::Wrench computeID(); 74 | KDL::Twist computeFD(const KDL::Wrench &Fb) const; 75 | 76 | 77 | private: 78 | 79 | double mass_; 80 | double mu_; 81 | 82 | KDL::Frame F_b_; 83 | KDL::Wrench W_b_; 84 | KDL::Twist V_b_; 85 | KDL::Twist s_V_b_; 86 | Eigen::Matrix N_; 87 | Eigen::Matrix C_; 88 | Eigen::Matrix g_; 89 | Eigen::Matrix I_; 90 | Eigen::Matrix massMatrix; 91 | Eigen::Matrix G_; 92 | Eigen::Matrix Go_; 93 | std::vector contacts_; 94 | }; 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /kdl_ros_control/include/kdl_ros_control/kdl_planner.h: -------------------------------------------------------------------------------- 1 | #ifndef KDLPlanner_H 2 | #define KDLPlanner_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "Eigen/Dense" 17 | 18 | struct trajectory_point{ 19 | Eigen::Vector3d pos = Eigen::Vector3d::Zero(); 20 | Eigen::Vector3d vel = Eigen::Vector3d::Zero(); 21 | Eigen::Vector3d acc = Eigen::Vector3d::Zero(); 22 | }; 23 | 24 | class KDLPlanner 25 | { 26 | 27 | public: 28 | 29 | KDLPlanner(double _maxVel, double _maxAcc); 30 | void CreateTrajectoryFromFrames(std::vector &_frames, 31 | double _radius, double _eqRadius); 32 | void createCircPath(KDL::Frame &_F_start, 33 | KDL::Vector &_V_centre, 34 | KDL::Vector &_V_base_p, 35 | KDL::Rotation &_R_base_end, 36 | double alpha, 37 | double eqradius); 38 | 39 | KDL::Trajectory* getTrajectory(); 40 | 41 | ////////////////////////////////// 42 | KDLPlanner(double _trajDuration, double _accDuration, 43 | Eigen::Vector3d _trajInit, Eigen::Vector3d _trajEnd); 44 | trajectory_point compute_trajectory(double time); 45 | 46 | private: 47 | 48 | KDL::Path_RoundedComposite* path_; 49 | KDL::Path_Circle* path_circle_; 50 | KDL::VelocityProfile* velpref_; 51 | KDL::Trajectory* traject_; 52 | 53 | ////////////////////////////////// 54 | double trajDuration_, accDuration_; 55 | Eigen::Vector3d trajInit_, trajEnd_; 56 | trajectory_point p; 57 | 58 | }; 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /kdl_ros_control/lib/liborocos-kdl.so: -------------------------------------------------------------------------------- 1 | liborocos-kdl.so.1.4 -------------------------------------------------------------------------------- /kdl_ros_control/lib/liborocos-kdl.so.1.4: -------------------------------------------------------------------------------- 1 | liborocos-kdl.so.1.4.0 -------------------------------------------------------------------------------- /kdl_ros_control/lib/liborocos-kdl.so.1.4.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/kdl_ros_control/lib/liborocos-kdl.so.1.4.0 -------------------------------------------------------------------------------- /kdl_ros_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kdl_ros_control 4 | 0.0.0 5 | The kdl_ros_control package 6 | 7 | 8 | 9 | 10 | mario selvaggio 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | catkin 31 | 32 | roscpp 33 | rospy 34 | std_msgs 35 | geometry_msgs 36 | message_generation 37 | kdl_parser 38 | 39 | eigen_conversions 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /kdl_ros_control/src/kdl_object_test.cpp: -------------------------------------------------------------------------------- 1 | #include "kdl_ros_control/kdl_object.h" 2 | 3 | int main(int argc, char **argv) 4 | { 5 | double frictionCoeff = 0.2, m = 0.1; 6 | Eigen::Matrix3d I = 1e-6*Eigen::Matrix3d::Identity(); 7 | std::vector contacts(4); 8 | contacts.at(0).p = KDL::Vector(-0.05, 0.05,-0.05); 9 | contacts.at(1).p = KDL::Vector(0.05, 0.05,-0.05); 10 | contacts.at(2).p = KDL::Vector(0.05,-0.05,-0.05); 11 | contacts.at(3).p = KDL::Vector(-0.05,-0.05,-0.05); 12 | KDLObject obj(m, I, contacts, frictionCoeff); 13 | 14 | KDL::Wrench F_b = KDL::Wrench(KDL::Vector(0,0,10), KDL::Vector(0,0,0)); 15 | obj.computeContactForces(F_b); 16 | 17 | 18 | // obj.setV_b(KDL::Twist(KDL::Vector(0,1,1),KDL::Vector(1,0,0))); 19 | // obj.setFrame(KDL::Frame(KDL::Rotation::Rot(KDL::Vector(1,0,0),M_PI/6.0),KDL::Vector(0,0,0))); 20 | // F_b = obj.computeID(); 21 | // std::cout << "ID: " << F_b.force.x() << " " 22 | // << F_b.force.y() << " " 23 | // << F_b.force.z() << " " 24 | // << F_b.torque.x() << " " 25 | // << F_b.torque.y() << " " 26 | // << F_b.torque.z() << " " 27 | // << std::endl; 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /kdl_ros_control/src/kdl_planner.cpp: -------------------------------------------------------------------------------- 1 | #include "kdl_ros_control/kdl_planner.h" 2 | 3 | KDLPlanner::KDLPlanner(double _maxVel, double _maxAcc) 4 | { 5 | velpref_ = new KDL::VelocityProfile_Trap(_maxVel,_maxAcc); 6 | } 7 | 8 | KDLPlanner::KDLPlanner(double _trajDuration, double _accDuration, Eigen::Vector3d _trajInit, Eigen::Vector3d _trajEnd) 9 | { 10 | trajDuration_ = _trajDuration; 11 | accDuration_ = _accDuration; 12 | trajInit_ = _trajInit; 13 | trajEnd_ = _trajEnd; 14 | } 15 | 16 | void KDLPlanner::CreateTrajectoryFromFrames(std::vector &_frames, 17 | double _radius, double _eqRadius 18 | ) 19 | { 20 | path_ = new KDL::Path_RoundedComposite(_radius,_eqRadius,new KDL::RotationalInterpolation_SingleAxis()); 21 | 22 | for (unsigned int i = 0; i < _frames.size(); i++) 23 | { 24 | path_->Add(_frames[i]); 25 | } 26 | path_->Finish(); 27 | 28 | velpref_->SetProfile(0,path_->PathLength()); 29 | traject_ = new KDL::Trajectory_Segment(path_, velpref_); 30 | } 31 | 32 | void KDLPlanner::createCircPath(KDL::Frame &_F_start, 33 | KDL::Vector &_V_centre, 34 | KDL::Vector& _V_base_p, 35 | KDL::Rotation& _R_base_end, 36 | double alpha, 37 | double eqradius 38 | ) 39 | { 40 | KDL::RotationalInterpolation_SingleAxis* otraj; 41 | otraj = new KDL::RotationalInterpolation_SingleAxis(); 42 | otraj->SetStartEnd(_F_start.M,_R_base_end); 43 | path_circle_ = new KDL::Path_Circle(_F_start, 44 | _V_centre, 45 | _V_base_p, 46 | _R_base_end, 47 | alpha, 48 | otraj, 49 | eqradius); 50 | velpref_->SetProfile(0,path_circle_->PathLength()); 51 | traject_ = new KDL::Trajectory_Segment(path_circle_, velpref_); 52 | } 53 | 54 | KDL::Trajectory* KDLPlanner::getTrajectory() 55 | { 56 | return traject_; 57 | } 58 | 59 | trajectory_point KDLPlanner::compute_trajectory(double time) 60 | { 61 | /* trapezoidal velocity profile with accDuration_ acceleration time period and trajDuration_ total duration. 62 | time = current time 63 | trajDuration_ = final time 64 | accDuration_ = acceleration time 65 | trajInit_ = trajectory initial point 66 | trajEnd_ = trajectory final point */ 67 | 68 | trajectory_point traj; 69 | 70 | Eigen::Vector3d ddot_traj_c = -1.0/(std::pow(accDuration_,2)-trajDuration_*accDuration_)*(trajEnd_-trajInit_); 71 | 72 | if(time <= accDuration_) 73 | { 74 | traj.pos = trajInit_ + 0.5*ddot_traj_c*std::pow(time,2); 75 | traj.vel = ddot_traj_c*time; 76 | traj.acc = ddot_traj_c; 77 | } 78 | else if(time <= trajDuration_-accDuration_) 79 | { 80 | traj.pos = trajInit_ + ddot_traj_c*accDuration_*(time-accDuration_/2); 81 | traj.vel = ddot_traj_c*accDuration_; 82 | traj.acc = Eigen::Vector3d::Zero(); 83 | } 84 | else 85 | { 86 | traj.pos = trajEnd_ - 0.5*ddot_traj_c*std::pow(trajDuration_-time,2); 87 | traj.vel = ddot_traj_c*(trajDuration_-time); 88 | traj.acc = -ddot_traj_c; 89 | } 90 | 91 | return traj; 92 | 93 | } 94 | -------------------------------------------------------------------------------- /models/cuboid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(cuboid) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 9 | install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 10 | -------------------------------------------------------------------------------- /models/cuboid/meshes/collision/cube.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/cuboid/meshes/collision/cube.stl -------------------------------------------------------------------------------- /models/cuboid/meshes/visual/cube.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/cuboid/meshes/visual/cube.stl -------------------------------------------------------------------------------- /models/cuboid/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cuboid 4 | 1.0.0 5 | This package contains the model files for cuboid object 6 | 7 | Mario Selvaggio 8 | Mario Selvaggio 9 | 10 | GPL 11 | catkin 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /models/cuboid/urdf/cuboid.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | true 9 | Gazebo/Red 10 | 1 11 | 1 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /models/cuboid/urdf/cuboid.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /models/cuboid/urdf/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /models/lbr_iiwa/.gitignore: -------------------------------------------------------------------------------- 1 | # Lines that start with '#' are comments. 2 | 3 | # Backup and data files 4 | eg/eg.* 5 | *~ 6 | *.bak 7 | *.off 8 | *.x 9 | x.* 10 | r 11 | x 12 | x? 13 | r? 14 | 15 | #Misc 16 | 17 | lbr_fri/fri 18 | 19 | # patch results 20 | *.rej 21 | 22 | # Build products 23 | tmp/* 24 | build/Makefile* 25 | build/CMakeFiles 26 | build/CMakeCache.txt 27 | build/*/Makefile* 28 | build/*/object_script* 29 | build/*/Release/* 30 | build/*/Debug/* 31 | build/*/MinSizeRel/* 32 | build/*/RelWithDebInfo/* 33 | build 34 | 35 | *# 36 | *.# 37 | *.obj 38 | *.urdf 39 | *.dae 40 | .stl 41 | *.zip 42 | *.a 43 | *.dll 44 | *.lib 45 | *.exe 46 | *.bag 47 | *.o 48 | *.md5sum 49 | 50 | # Qt files 51 | *.pro.user 52 | 53 | # DevStudio files 54 | *.bsc 55 | *.ncb 56 | *.pdb 57 | *.suo 58 | *.user 59 | *.ilk 60 | 61 | # CVS files 62 | CVS/* 63 | */CVS/* 64 | */*/CVS/* 65 | */*/*/CVS/* 66 | *.cvsignore 67 | 68 | # Other files 69 | working/ 70 | Status API Training Shop Blog About 71 | © 2014 GitHub, Inc. Terms Privacy Security Contact 72 | 73 | -------------------------------------------------------------------------------- /models/lbr_iiwa/README.md: -------------------------------------------------------------------------------- 1 | Urdf & Gazebo files for a KUKA LBR iiwa R820 with electrical media flange 2 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_control/launch/lbr_iiwa_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 14 | 15 | 16 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lbr_iiwa_control 4 | 0.0.0 5 | The lbr_iiwa_control package 6 | 7 | Robert Krug 8 | 9 | BSD 10 | 11 | Robert Krug 12 | 13 | catkin 14 | 15 | controller_manager 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/gazebo/gazebo.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | /lbr_iiwa 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/launch/lbr_iiwa_online.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/launch/lbr_iiwa_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_0.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_1.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_2.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_3.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_4.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_5.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_6.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/coarse/link_7.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/link_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/link_0.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/link_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/link_1.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/link_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/link_2.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/link_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/link_3.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/link_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/link_4.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/link_5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/link_5.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/link_6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/link_6.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/meshes/link_7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/lbr_iiwa/lbr_iiwa_description/meshes/link_7.stl -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lbr_iiwa_description 4 | 0.0.0 5 | The rrbot_description package 6 | 7 | Robert Krug 8 | 9 | BSD 10 | 11 | Robert Krug 12 | 13 | catkin 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/urdf/lbr_iiwa.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | Gazebo/Grey 10 | 0.0 11 | 0.0 12 | 13 | 14 | 15 | 16 | Gazebo/Orange 17 | 0.0 18 | 0.0 19 | 20 | 21 | 22 | 23 | Gazebo/Orange 24 | 0.0 25 | 0.0 26 | 27 | 28 | 29 | 30 | Gazebo/Orange 31 | 0.0 32 | 0.0 33 | 34 | 35 | 36 | 37 | 38 | Gazebo/Orange 39 | 0.0 40 | 0.0 41 | 42 | 43 | 44 | 45 | Gazebo/Orange 46 | 0.0 47 | 0.0 48 | 49 | 50 | 51 | 52 | Gazebo/Orange 53 | 0.0 54 | 0.0 55 | 56 | 57 | 58 | 59 | Gazebo/Grey 60 | 0.0 61 | 0.0 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/urdf/lbr_iiwa.transmission.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | transmission_interface/SimpleTransmission 8 | 9 | ${hardware_interface} 10 | 11 | 12 | ${hardware_interface} 13 | 1 14 | 15 | 16 | 17 | 18 | transmission_interface/SimpleTransmission 19 | 20 | ${hardware_interface} 21 | 22 | 23 | ${hardware_interface} 24 | 1 25 | 26 | 27 | 28 | 29 | transmission_interface/SimpleTransmission 30 | 31 | ${hardware_interface} 32 | 33 | 34 | ${hardware_interface} 35 | 1 36 | 37 | 38 | 39 | 40 | transmission_interface/SimpleTransmission 41 | 42 | ${hardware_interface} 43 | 44 | 45 | ${hardware_interface} 46 | 1 47 | 48 | 49 | 50 | 51 | transmission_interface/SimpleTransmission 52 | 53 | ${hardware_interface} 54 | 55 | 56 | ${hardware_interface} 57 | 1 58 | 59 | 60 | 61 | 62 | transmission_interface/SimpleTransmission 63 | 64 | ${hardware_interface} 65 | 66 | 67 | ${hardware_interface} 68 | 1 69 | 70 | 71 | 72 | 73 | transmission_interface/SimpleTransmission 74 | 75 | ${hardware_interface} 76 | 77 | 78 | ${hardware_interface} 79 | 1 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/urdf/lbr_iiwa.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/urdf/lbr_iiwa_ee.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/urdf/lbr_iiwa_urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/urdf/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_description/urdf/utilities.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | 17 | 18 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_gazebo/launch/lbr_iiwa_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lbr_iiwa_gazebo 4 | 0.0.0 5 | The lbr_iiwa_gazebo package 6 | 7 | Robert Krug 8 | 9 | Robert Krug 10 | 11 | BSD 12 | 13 | 14 | catkin 15 | 16 | lbr_iiwa_description 17 | gazebo_ros 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_gazebo/worlds/lbr_iiwa.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | 11 | 12 | model://sun 13 | 14 | 15 | 16 | 17 | 18 | 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 19 | orbit 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_java/JointTargetsListner.java: -------------------------------------------------------------------------------- 1 | package application; 2 | 3 | import java.io.IOException; 4 | import java.util.concurrent.Semaphore; 5 | import java.util.concurrent.locks.Condition; 6 | import java.util.concurrent.locks.Lock; 7 | import java.util.concurrent.locks.ReentrantLock; 8 | 9 | import sun.awt.Mutex; 10 | 11 | import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; 12 | import com.kuka.roboticsAPI.controllerModel.Controller; 13 | import com.kuka.roboticsAPI.deviceModel.JointPosition; 14 | import com.kuka.roboticsAPI.deviceModel.LBR; 15 | 16 | public class JointTargetsListner extends Thread { 17 | private String hostname; 18 | int SIZE = 7; 19 | private Client mylink_send, mylink_recv; 20 | private boolean quit; 21 | private JointPosition jp; 22 | private double joint_targets[], joint_states[]; 23 | private static final Lock lockObj = new ReentrantLock(); 24 | private static final Condition cond = lockObj.newCondition(); 25 | //private final Semaphore sem; 26 | 27 | public JointPosition getJP(JointPosition js) {// throws InterruptedException { 28 | lockObj.lock(); 29 | JointPosition jp = new JointPosition(joint_targets); 30 | for(int j=0; j 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_launch/launch/lbr_iiwa_gazebo_effort_pos_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_launch/launch/lbr_iiwa_gazebo_forward_pos_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_launch/launch/lbr_iiwa_gazebo_forward_vel_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /models/lbr_iiwa/lbr_iiwa_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lbr_iiwa_launch 4 | 0.0.0 5 | The lbr_iiwa_control package 6 | 7 | Robert Krug 8 | 9 | BSD 10 | 11 | Robert Krug 12 | 13 | catkin 14 | 15 | lbr_iiwa_gazebo 16 | lbr_iiwa_control 17 | controller_manager 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /models/lbr_iiwa/license.dat: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Robert Krug & Todor Stoyanov, AASS Research Center, Orebro University, Sweden 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | 6 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | 8 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | 10 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 11 | -------------------------------------------------------------------------------- /models/planar_end_eff/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(planar_end_eff) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 9 | install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 10 | -------------------------------------------------------------------------------- /models/planar_end_eff/meshes/collision/planar_end_eff_collision.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/planar_end_eff/meshes/collision/planar_end_eff_collision.stl -------------------------------------------------------------------------------- /models/planar_end_eff/meshes/visual/planar_end_eff_visual.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/planar_end_eff/meshes/visual/planar_end_eff_visual.blend -------------------------------------------------------------------------------- /models/planar_end_eff/meshes/visual/planar_end_eff_visual.blend1: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/planar_end_eff/meshes/visual/planar_end_eff_visual.blend1 -------------------------------------------------------------------------------- /models/planar_end_eff/meshes/visual/planar_end_eff_visual.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/models/planar_end_eff/meshes/visual/planar_end_eff_visual.stl -------------------------------------------------------------------------------- /models/planar_end_eff/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | planar_end_eff 4 | 1.0.0 5 | This package contains the model files for the lbr planar_end_eff 6 | 7 | Mario Selvaggio 8 | Mario Selvaggio 9 | 10 | GPL 11 | catkin 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /models/planar_end_eff/urdf/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /models/planar_end_eff/urdf/planar_end_eff.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | Gazebo/DarkGrey 9 | 0.5 10 | 0.5 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /models/planar_end_eff/urdf/planar_end_eff.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | true 39 | true 40 | 41 | 42 | 43 | 44 | 45 | 100.0 46 | ft_sensor_topic 47 | planar_end_eff_fixed_base_joint 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /nonprehensile-object-transp-ros-pkg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nonprehensile-object-transp-iiwa 4 | 0.0.0 5 | The nonprehensile-object-transp-iiwa package 6 | 7 | 8 | 9 | 10 | mario selvaggio 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | catkin 31 | 32 | roscpp 33 | rospy 34 | std_msgs 35 | kdl_ros_control 36 | geometry_msgs 37 | message_generation 38 | 39 | eigen_conversions 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /play_video_figure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/play_video_figure.png -------------------------------------------------------------------------------- /rodyman.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prisma-lab/nonprehensile-object-transp/99d9b57e09f95579df5bcb239890cac82b7e7296/rodyman.png --------------------------------------------------------------------------------