30 const std::vector<std::string>& endpoints,
34 const double x_dot_trans_max,
35 const double x_dot_rot_max,
40 q_dot_max_(nr_of_jnts),
51 for (
size_t i = 0; i < endpoints.size(); i++)
68 for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
72 for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
75 Frames::iterator f_it =
frames_.find(f_des_it->first);
76 Twists::iterator delta_twists_it =
delta_twists_.find(f_des_it->first);
85 delta_twists_it->second =
twist_;
103 for (
unsigned int j = 0; j <
q_min_.
rows(); j++)
107 else if (q_out(j) >
q_max_(j))
120 double rel_os, rel_os_max = 0.0;
121 bool max_exceeded =
false;
123 for (
unsigned int i = 0; i <
q_dot_.
rows(); i++)
129 if ( rel_os > rel_os_max )
138 if ( rel_os > rel_os_max)
146 if ( max_exceeded ==
true )
155 double x_dot_trans, x_dot_rot;
161 if ( x_dot_trans > x_dot_rot )
166 else if ( x_dot_rot > x_dot_trans )
void enforceJointVelLimits()
virtual int JntToCart(const JntArray &q_in, Frame &p_out, std::string segmentName)=0
This abstract class encapsulates a solver for the forward position kinematics for a KDL::Tree...
unsigned int rows() const
Vector vel
The velocity of that point.
TreeFkSolverPos & fksolver_
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
TreeIkSolverVel & iksolver_
This class represents an fixed size array containing joint values of a KDL::Chain.
~TreeIkSolverPos_Online()
TreeIkSolverPos_Online(const double &nr_of_jnts, const std::vector< std::string > &endpoints, const JntArray &q_min, const JntArray &q_max, const JntArray &q_dot_max, const double x_dot_trans_max, const double x_dot_rot_max, TreeFkSolverPos &fksolver, TreeIkSolverVel &iksolver)
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
void enforceCartVelLimits()
Vector rot
The rotational velocity of that point.
std::map< std::string, Frame > Frames
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
This abstract class encapsulates the inverse velocity solver for a KDL::Tree.
virtual double CartToJnt(const JntArray &q_in, const Frames &p_in, JntArray &q_out)
virtual double CartToJnt(const JntArray &q_in, const Twists &v_in, JntArray &qdot_out)=0
void Multiply(const JntArray &src, const double &factor, JntArray &dest)