KDL  1.4.0
chainjnttojacsolver.hpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
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 {
42  {
43  public:
44 
45  explicit ChainJntToJacSolver(const Chain& chain);
46  virtual ~ChainJntToJacSolver();
58  virtual int JntToJac(const JntArray& q_in, Jacobian& jac, int segmentNR=-1);
59 
65  int setLockedJoints(const std::vector<bool> locked_joints);
66 
68  virtual void updateInternalDataStructures();
69 
70  private:
71  const Chain& chain;
74  std::vector<bool> locked_joints_;
75  };
76 }
77 #endif
78 
KDL::ChainJntToJacSolver::ChainJntToJacSolver
ChainJntToJacSolver(const Chain &chain)
Definition: chainjnttojacsolver.cpp:26
chain.hpp
frames.hpp
KDL::Jacobian::columns
unsigned int columns() const
Definition: jacobian.cpp:75
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:695
KDL::Jacobian::setColumn
void setColumn(unsigned int i, const Twist &t)
Definition: jacobian.cpp:149
KDL::ChainJntToJacSolver::chain
const Chain & chain
Definition: chainjnttojacsolver.hpp:71
KDL::JntArray
Definition: jntarray.hpp:70
KDL::Segment::getJoint
const Joint & getJoint() const
Request the joint of the segment.
Definition: segment.hpp:118
jntarray.hpp
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:572
KDL::SolverI::E_NOT_UP_TO_DATE
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL
Definition: articulatedbodyinertia.cpp:28
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Request the total number of segments in the chain.
Definition: chain.hpp:76
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::ChainJntToJacSolver::T_tmp
Frame T_tmp
Definition: chainjnttojacsolver.hpp:73
KDL::SolverI::E_OUT_OF_RANGE
@ E_OUT_OF_RANGE
Requested index out of range.
Definition: solveri.hpp:103
KDL::JntArray::rows
unsigned int rows() const
Returns the number of rows (size) of the array.
Definition: jntarray.cpp:72
KDL::ChainJntToJacSolver::JntToJac
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int segmentNR=-1)
Calculate the jacobian expressed in the base frame of the chain, with reference point at the end effe...
Definition: chainjnttojacsolver.cpp:48
KDL::Chain::getSegment
const Segment & getSegment(unsigned int nr) const
Request the nr'd segment of the chain.
Definition: chain.cpp:68
KDL::SolverI::E_SIZE_MISMATCH
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
KDL::ChainJntToJacSolver::~ChainJntToJacSolver
virtual ~ChainJntToJacSolver()
Definition: chainjnttojacsolver.cpp:34
KDL::Segment::pose
Frame pose(const double &q) const
Request the pose of the segment, given the joint position q.
Definition: segment.cpp:57
KDL::ChainJntToJacSolver::setLockedJoints
int setLockedJoints(const std::vector< bool > locked_joints)
Definition: chainjnttojacsolver.cpp:38
KDL::Frame
Definition: frames.hpp:570
solveri.hpp
KDL::Segment::twist
Twist twist(const double &q, const double &qdot) const
Request the 6D-velocity of the tip of the segment, given the joint position q and the joint velocity ...
Definition: segment.cpp:62
KDL::Joint::None
@ None
Definition: joint.hpp:47
KDL::ChainJntToJacSolver::updateInternalDataStructures
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainjnttojacsolver.cpp:31
KDL::ChainJntToJacSolver::t_tmp
Twist t_tmp
Definition: chainjnttojacsolver.hpp:72
KDL::ChainJntToJacSolver::locked_joints_
std::vector< bool > locked_joints_
Definition: chainjnttojacsolver.hpp:74
KDL::SolverI
Solver interface supporting storage and description of the latest error.
Definition: solveri.hpp:85
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
KDL::changeRefPoint
bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:90
KDL::Joint::getType
const JointType & getType() const
Request the type of the joint.
Definition: joint.hpp:159
chainjnttojacsolver.hpp
KDL::ChainJntToJacSolver
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
Definition: chainjnttojacsolver.hpp:42
KDL::SetToZero
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:80
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91
jacobian.hpp
KDL::Chain
Definition: chain.hpp:35
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Request the total number of joints in the chain.
Definition: chain.hpp:71
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
KDL::Jacobian
Definition: jacobian.hpp:37