IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
return FrameVel(
addDelta(a.M,da.rot,dt),
addDelta(a.p,da.vel,dt)
);
}
IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) {
return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt));
}
IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) {
return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt));
}
double TreeIkSolverPos_Online::CartToJnt_it(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
{
assert(q_out.rows() == q_in.rows());
assert(q_out_old_.rows() == q_in.rows());
assert(q_dot_.rows() == q_in.rows());
assert(q_dot_old_.rows() == q_in.rows());
assert(q_dot_new_.rows() == q_in.rows());
q_out = q_in;
SetToZero(q_dot_);
SetToZero(q_dot_old_);
SetToZero(q_dot_new_);
twist_ = Twist::Zero();
// First check, if all elements in p_in are available
unsigned int nr_of_endeffectors = 0;
nr_of_still_endeffectors_ = 0;
low_pass_adj_factor_ = 0.0;
for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
{
if(frames_.find(f_des_it->first)==frames_.end())
return -2;
else
{
/*
Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
old_twists_it->second = Twist::Zero();
Frames::iterator f_0_it = frames_0_.find(f_des_it->first);
*/
/*
Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
old_twists_it->second = diff(f_old_it->second, f_des_it->second);
if (){};
*/
Frames::iterator f_old_it = p_in_old_.find(f_des_it->first);
Frames::iterator f_des_pos_l_it = frames_pos_lim_.find(f_des_it->first);
twist_ = diff(f_old_it->second, f_des_it->second);
/*
if(sqrt(pow(twist_.vel.x(), 2) + pow(twist_.vel.y(), 2) + pow(twist_.vel.z(), 2)) < x_dot_trans_min_)
{
f_des_pos_l_it->second.p = addDelta(f_old_it->second.p, (0.1 * x_dot_trans_max_ * twist_.vel));
std::cout << "old position is used." << std::endl;
}
else
f_des_pos_l_it->second.p = f_des_it->second.p;
if(sqrt(pow(twist_.rot.x(), 2) + pow(twist_.rot.y(), 2) + pow(twist_.rot.z(), 2)) < x_dot_rot_min_)
{
f_des_pos_l_it->second.M = addDelta(f_old_it->second.M, (0.1 * x_dot_rot_max_ * twist_.rot));
std::cout << "old orientation is used." << std::endl;
}
else
f_des_pos_l_it->second.M = f_des_it->second.M;
*/
f_des_pos_l_it->second.p = f_des_it->second.p;
f_des_pos_l_it->second.M = f_des_it->second.M;
Frames::iterator f_des_vel_l_it = frames_vel_lim_.find(f_des_it->first);
fksolver_.JntToCart(q_in, f_des_vel_l_it->second, f_des_it->first);
twist_ = diff(f_des_vel_l_it->second, f_des_pos_l_it->second);
f_des_vel_l_it->second = addDelta(f_des_vel_l_it->second, twist_);
nr_of_endeffectors++;
}
}
if(nr_of_still_endeffectors_ == nr_of_endeffectors)
{
small_task_space_movement_ = true;
}
else
small_task_space_movement_ = false;
unsigned int k=0;
double res = 0.0;
while(++k <= maxiter_)
{
//for (Frames::const_iterator f_des_it=p_in.begin(); f_des_it!=p_in.end(); ++f_des_it)
for (Frames::const_iterator f_des_it=frames_vel_lim_.begin(); f_des_it!=frames_vel_lim_.end(); ++f_des_it)
{
// Get all iterators for this endpoint
Frames::iterator f_it = frames_.find(f_des_it->first);
Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);
Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
Frames::iterator f_0_it = frames_vel_lim_.find(f_des_it->first);
fksolver_.JntToCart(q_out, f_it->second, f_it->first);
// Checks, if the current overall twist exceeds the maximum translational and/or rotational velocity.
// If so, the velocities of the overall twist get scaled and a new current twist is calculated.
delta_twists_it->second = diff(f_it->second, f_des_it->second);
old_twists_it->second = diff(f_0_it->second, f_it->second);
enforceCartVelLimits_it(old_twists_it->second, delta_twists_it->second);
}
res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);
if (res < eps_)
{
break;
//return res;
}
// Checks, if joint velocities (q_dot_) exceed their maximum velocities and scales them, if necessary
//Subtract(q_out, q_in, q_dot_old_);
//enforceJointVelLimits_it(q_dot_old_, q_dot_);
Subtract(q_out, q_in, q_dot_old_);
Add(q_dot_old_, q_dot_, q_dot_);
enforceJointVelLimits();
Subtract(q_dot_, q_dot_old_, q_dot_);
// Integrate
Add(q_out, q_dot_, q_out);
// Limit joint positions
for (unsigned int j = 0; j < q_min_.rows(); ++j)
{
if (q_out(j) < q_min_(j))
q_out(j) = q_min_(j);
else if (q_out(j) > q_max_(j))
q_out(j) = q_max_(j);
}
}
Subtract(q_out, q_in, q_dot_);
Add(q_in, q_dot_, q_out);
filter(q_dot_, q_out, q_out_old_);
q_out_old_ = q_out;
p_in_old_ = p_in;
if (k <= maxiter_)
return res;
else
return -3;
}
