Go to the documentation of this file.
34 const double x_dot_trans_max,
35 const double x_dot_rot_max,
40 q_dot_(static_cast<unsigned int>(nr_of_jnts))
48 for (
size_t i = 0; i < endpoints.
size(); i++)
65 for(Frames::const_iterator f_des_it=p_in.
begin();f_des_it!=p_in.
end();++f_des_it)
69 for (Frames::const_iterator f_des_it=p_in.
begin();f_des_it!=p_in.
end();++f_des_it)
72 Frames::iterator f_it =
frames_.
find(f_des_it->first);
82 delta_twists_it->second =
twist_;
100 for (
unsigned int j = 0; j <
q_min_.
rows(); j++)
104 else if (q_out(j) >
q_max_(j))
117 double rel_os, rel_os_max = 0.0;
118 bool max_exceeded =
false;
120 for (
unsigned int i = 0; i <
q_dot_.
rows(); i++)
126 if ( rel_os > rel_os_max )
135 if ( rel_os > rel_os_max)
143 if ( max_exceeded ==
true )
152 double x_dot_trans, x_dot_rot;
158 if ( x_dot_trans > x_dot_rot )
163 else if ( x_dot_rot > x_dot_trans )
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
Vector rot
The rotational velocity of that point.
virtual double CartToJnt(const JntArray &q_in, const Frames &p_in, JntArray &q_out)
This abstract class encapsulates a solver for the forward position kinematics for a KDL::Tree.
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
This abstract class encapsulates the inverse velocity solver for a KDL::Tree.
void Multiply(const JntArray &src, const double &factor, JntArray &dest)
TreeIkSolverVel & iksolver_
~TreeIkSolverPos_Online()
unsigned int rows() const
virtual double CartToJnt(const JntArray &q_in, const Twists &v_in, JntArray &qdot_out)=0
virtual int JntToCart(const JntArray &q_in, Frame &p_out, std::string segmentName)=0
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)
Vector vel
The velocity of that point.
void enforceJointVelLimits()
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
TreeFkSolverPos & fksolver_
void enforceCartVelLimits()