void AdmittanceController::update(const Eigen::VectorXd& tau, Eigen::VectorXd& qdot_reference, const KDL::JntArray& q_current, Eigen::VectorXd& q_reference) {
// Update filters --> as a result desired velocities are known
for (int i = 0; i<tau.size(); i++) {
/// Check input values (and create segfault to debug)
if (!(fabs(tau(i)) < 10000.0 && fabs(qdot_reference_previous_(i)) < 10000.0) ) {
std::cout << "tauin: " << tau(i) << ", qdot: " << qdot_reference_previous_(i) << std::endl;
assert(false);
} else {
qdot_reference(i) = b_(0,i) * tau(i);
qdot_reference(i) += b_(1,i) * tau_previous_(i);
qdot_reference(i) -= a_(1,i) * qdot_reference_previous_(i);
qdot_reference(i) *= k_(i);
// Integrate desired velocities and limit outputs
q_reference(i) = std::min(q_max_(i),std::max(q_min_(i),q_current(i)+Ts_*qdot_reference(i)));
}
}
tau_previous_ = tau;
qdot_reference_previous_ = qdot_reference;
//ROS_INFO("qdr = %f %f %f %f", qdot_reference(0), qdot_reference(1), qdot_reference(2), qdot_reference(3));
///ROS_INFO("qdrspindle = %f", qdot_reference(7));
}
double TreeIkSolverPos_Online::CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
{
assert(q_out.rows() == q_in.rows());
assert(q_dot_.rows() == q_out.rows());
q_out = q_in;
// First check, if all elements in p_in are available
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;
for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.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);
fksolver_.JntToCart(q_out, f_it->second, f_it->first);
twist_ = diff(f_it->second, f_des_it->second);
// Checks, if the twist (twist_) exceeds the maximum translational and/or rotational velocity
// And scales them, if necessary
enforceCartVelLimits();
delta_twists_it->second = twist_;
}
double res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);
// Checks, if joint velocities (q_dot_) exceed their maximum velocities and scales them, if necessary
enforceJointVelLimits();
// 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);
}
return res;
}
bool JacoIKSolver::initFromURDF(const std::string urdf, const std::string root_name,
const std::string tip_name, unsigned int max_iter, std::string error)
{
urdf::Model robot_model;
KDL::Tree tree;
if (!robot_model.initFile(urdf))
{
error += "Could not initialize robot model";
return false;
}
if (!kdl_parser::treeFromFile(urdf, tree))
{
error += "Could not initialize tree object";
return false;
}
if (tree.getSegment(root_name) == tree.getSegments().end())
{
error += "Could not find root link '" + root_name + "'.";
return false;
}
if (tree.getSegment(tip_name) == tree.getSegments().end())
{
error += "Could not find tip link '" + tip_name + "'.";
return false;
}
if (!tree.getChain(root_name, tip_name, chain_))
{
error += "Could not initialize chain object";
return false;
}
// Get the joint limits from the robot model
q_min_.resize(chain_.getNrOfJoints());
q_max_.resize(chain_.getNrOfJoints());
q_seed_.resize(chain_.getNrOfJoints());
joint_names_.resize(chain_.getNrOfJoints());
unsigned int j = 0;
for(unsigned int i = 0; i < chain_.getNrOfSegments(); ++i)
{
const KDL::Joint& kdl_joint = chain_.getSegment(i).getJoint();
if (kdl_joint.getType() != KDL::Joint::None)
{
// std::cout << chain_.getSegment(i).getName() << " -> " << kdl_joint.getName() << " -> " << chain_.getSegment(i + 1).getName() << std::endl;
boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(kdl_joint.getName());
if (joint && joint->limits)
{
q_min_(j) = joint->limits->lower;
q_max_(j) = joint->limits->upper;
q_seed_(j) = (q_min_(j) + q_max_(j)) / 2;
}
else
{
q_min_(j) = -1e9;
q_max_(j) = 1e9;
q_seed_(j) = 0;
}
joint_names_[j] = kdl_joint.getName();
++j;
}
}
;
fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_));
ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(chain_));
ik_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(chain_, q_min_, q_max_, *fk_solver_, *ik_vel_solver_, max_iter));
std::cout << "Using normal solver" << std::endl;
jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));
return true;
}
void AdmittanceController::initialize(const double Ts, const KDL::JntArray& q_min, const KDL::JntArray& q_max, const std::vector<double>& mass, const std::vector<double>& damping) {
// Resize relevant parameters
uint num_joints = 15;
Ts_ = Ts;
// ToDo: Make variables (use M and D as inputs?)
m_.resize(num_joints);
d_.resize(num_joints);
k_.resize(num_joints);
om_.resize(num_joints);
a_.resize(2,num_joints);
b_.resize(2,num_joints);
tau_previous_.resize(num_joints);
qdot_reference_previous_.resize(num_joints);
q_min_.resize(num_joints);
q_max_.resize(num_joints);
// Set parameters
/*(hardcoded)
for (uint i = 0; i<num_joints; i++) {
m_(i) = 0.1;
d_(i) = 1;
}
m_(7) = 1; // Torso
d_(7) = 10; // Torso
*/
for (uint i = 0; i < num_joints; i++) {
m_(i) = mass[i];
d_(i) = damping[i];
}
for (uint i = 0; i<num_joints; i++) {
k_(i) = 1/d_(i);
om_(i) = d_(i)/m_(i);
double wp = om_(i) + eps;
double alpha = wp/(tan(wp*Ts_/2));
double x1 = alpha/om_(i)+1;
double x2 = -alpha/om_(i)+1;
// Numerator and denominator of the filter
a_(0,i) = 1;
a_(1,i) = x2 / x1;
b_(0,i) = 1 / x1;
b_(1,i) = 1 / x1;
///ROS_INFO("a %i = %f, %f, b %i = %f, %f", i, a_(0,i), a_(1,i), i, b_(0,1), b_(1,i));
// Set previous in- and outputs to zero
tau_previous_(i) = 0;
qdot_reference_previous_(i) = 0;
// Set joint limits
q_min_(i) = q_min(i);
q_max_(i) = q_max(i);
}
// Set inputs, states, outputs to zero
ROS_INFO("Admittance controller initialized");
}
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;
}
