void ITRCartesianImpedanceController::starting(const ros::Time& time)
{
KDL::Rotation cur_R(cart_handles_.at(0).getPosition(),
cart_handles_.at(1).getPosition(),
cart_handles_.at(2).getPosition(),
cart_handles_.at(4).getPosition(),
cart_handles_.at(5).getPosition(),
cart_handles_.at(6).getPosition(),
cart_handles_.at(8).getPosition(),
cart_handles_.at(9).getPosition(),
cart_handles_.at(10).getPosition());
KDL::Vector cur_p(cart_handles_.at(3).getPosition(),
cart_handles_.at(7).getPosition(),
cart_handles_.at(11).getPosition());
KDL::Frame cur_T( cur_R, cur_p );
x_ref_ = cur_T;
x_des_ = cur_T;
x_prev_ = cur_T;
x_prev_quat_ = Eigen::Quaterniond(Eigen::Matrix3d(x_prev_.M.data).transpose());
x_prev_quat_.normalize();
// Initial Cartesian stiffness
KDL::Stiffness k( 800.0, 800.0, 800.0, 50.0, 50.0, 50.0 );
k_des_ = k;
// Initial force/torque measure
KDL::Wrench w(KDL::Vector(0.0, 0.0, 0.0), KDL::Vector(0.0, 0.0, 0.0));
f_des_ = w;
std::vector<double> cur_T_FRI;
fromKDLtoFRI(x_des_, cur_T_FRI);
// forward commands to hwi
for(int c = 0; c < 30; ++c)
{
if(c < 12)
cart_handles_.at(c).setCommand(cur_T_FRI.at(c));
if(c > 11 && c < 18)
cart_handles_.at(c).setCommand(k_des_[c-12]);
if(c > 17 && c < 24)
cart_handles_.at(c).setCommand(d_des_[c-18]);
if(c > 23 && c < 30)
cart_handles_.at(c).setCommand(f_des_[c-24]);
}
}
void GimRasterizer::square_border(const std::vector<Point2d>& uv, const std::vector<Point3d>& xyz) {
ogf_assert(uv.size() == xyz.size()) ;
int count = uv.size() ;
const Point2d& uv0 = uv[0] ;
const Point2d& uv1 = uv[uv.size() - 1] ;
int varying_coord = ::fabs(uv1.x() - uv0.x()) > ::fabs(uv1.y() - uv0.y()) ? 0 : 1 ;
int fixed_coord = 1 - varying_coord ;
int w = target_->size(varying_coord) ;
int h = target_->size(fixed_coord) ;
int fixed_coord_val = int(double(h-1) * (0.5 * (uv0[fixed_coord] + uv1[fixed_coord]))) ;
bool flipped = (uv1[varying_coord] < uv0[varying_coord]) ;
bool swapped = (fixed_coord == 0) ;
// Flips the curvilinear absissa if needed
std::vector<double> s ;
if(flipped) {
for(unsigned int i=0; i<uv.size(); i++) {
s.push_back(1.0 - uv[i][varying_coord]) ;
}
} else {
for(unsigned int i=0; i<uv.size(); i++) {
s.push_back(uv[i][varying_coord]) ;
}
}
double ds = 1.0 / double(w - 1.0) ;
double cur_s = 0.0 ;
unsigned int cur_index = 0 ;
for(unsigned int i=0; i<w; i++) {
while(s[cur_index+1] < cur_s) {
cur_index++ ;
}
double local_ds = s[cur_index+1] - s[cur_index] ;
double local_s2 ;
if(::fabs(local_ds) < 1e-30) {
local_s2 = 1.0 ;
} else {
local_s2 = (cur_s - s[cur_index]) / local_ds ;
}
double local_s1 = 1.0 - local_s2 ;
const Point3d& p1 = xyz[cur_index] ;
const Point3d& p2 = xyz[cur_index+1] ;
Point3d cur_p(
local_s1 * p1.x() + local_s2 * p2.x(),
local_s1 * p1.y() + local_s2 * p2.y(),
local_s1 * p1.z() + local_s2 * p2.z()
) ;
if(swapped) {
set_pixel(fixed_coord_val, flipped ? w - 1 - i : i, cur_p) ;
} else {
set_pixel(flipped ? w - 1 - i : i, fixed_coord_val, cur_p) ;
}
cur_s += ds ;
}
set_pixel(uv[0], xyz[0]) ;
set_pixel(uv[uv.size() - 1], xyz[xyz.size() - 1]) ;
}
void ITRCartesianImpedanceController::update(const ros::Time& time, const ros::Duration& period)
{
// get current values
//std::cout << "Update current values" << std::endl;
KDL::Rotation cur_R(cart_handles_.at(0).getPosition(),
cart_handles_.at(1).getPosition(),
cart_handles_.at(2).getPosition(),
cart_handles_.at(4).getPosition(),
cart_handles_.at(5).getPosition(),
cart_handles_.at(6).getPosition(),
cart_handles_.at(8).getPosition(),
cart_handles_.at(9).getPosition(),
cart_handles_.at(10).getPosition());
KDL::Vector cur_p(cart_handles_.at(3).getPosition(),
cart_handles_.at(7).getPosition(),
cart_handles_.at(11).getPosition());
KDL::Frame cur_T( cur_R, cur_p );
x_cur_ = cur_T;
std::vector<double> cur_T_FRI;
// Cartesian speed limit for new positions:
// create quaternions for current and desired orientation, normalize them to get correct angular distance later!
x_cur_quat_ = Eigen::Matrix3d(x_cur_.M.data).transpose();
x_cur_quat_.normalize();
x_des_quat_ = Eigen::Matrix3d(x_des_.M.data).transpose();
x_des_quat_.normalize();
x_set_quat_ = x_des_quat_;
// get linear desired velocity
x_dot_.vel = (x_des_.p-x_set_.p) / period.toSec();
// limit linear velocity to desired position
double x_dot_vel_norm;
x_dot_vel_norm = x_dot_.vel.Norm();
if (x_dot_vel_norm > max_trans_speed_) {
x_dot_.vel = x_dot_.vel / x_dot_vel_norm * max_trans_speed_;
}
if (cmd_flag_) {
// Interpolate position
x_set_.p = x_prev_.p + (x_dot_.vel * period.toSec());
// Interpolate orientation with SLERP
double slerp_ratio = max_rot_speed_ / (x_des_quat_.angularDistance(x_prev_quat_)) * period.toSec();
slerp_ratio = std::min(slerp_ratio, 1.0);
x_set_quat_ = x_prev_quat_.slerp(slerp_ratio, x_des_quat_);
// convert quaternion to rot matrix
x_set_quat_.normalize(); // need to be normalized to get right rotation matrix
x_set_.M = KDL::Rotation::Quaternion(x_set_quat_.x(), x_set_quat_.y(), x_set_quat_.z(), x_set_quat_.w());
}
fromKDLtoFRI(x_set_, cur_T_FRI);
// forward commands to hwi
for(int c = 0; c < 30; ++c)
{
if(c < 12)
cart_handles_.at(c).setCommand(cur_T_FRI.at(c));
if(c > 11 && c < 18)
cart_handles_.at(c).setCommand(k_des_[c-12]);
if(c > 17 && c < 24)
cart_handles_.at(c).setCommand(d_des_[c-18]);
if(c > 23 && c < 30)
cart_handles_.at(c).setCommand(f_des_[c-24]);
}
x_prev_ = x_set_;
x_prev_quat_ = x_set_quat_;
}
bool ITRCartesianImpedanceController::init(hardware_interface::PositionCartesianInterface *robot, ros::NodeHandle &n)
{
ROS_INFO("THIS CONTROLLER USES THE TCP INFORMATION SET ON THE FRI SIDE... SO BE SURE YOU KNOW WHAT YOU ARE DOING!");
if (!ros::param::search(n.getNamespace(),"robot_name", robot_namespace_))
{
ROS_WARN_STREAM("ITRCartesianImpedanceController: No robot name found on parameter server ("<<n.getNamespace()<<"/robot_name), using the namespace...");
robot_namespace_ = n.getNamespace();
//return false;
}
if (!n.getParam("robot_name", robot_namespace_))
{
ROS_WARN_STREAM("ITRCartesianImpedanceController: Could not read robot name from parameter server ("<<n.getNamespace()<<"/robot_name), using the namespace...");
robot_namespace_ = n.getNamespace();
//return false;
}
// stuff to read KDL chain in order to get the transform between base_link and robot
if( !(KinematicChainControllerBase<hardware_interface::PositionCartesianInterface >::init(robot, n)) )
{
ROS_ERROR("Couldn't execute init of the KinematikChainController to get the KDL chain.");
return false;
}
KDL::Chain mount_chain;
//cout << "reading segment 0 name:" << endl;
kdl_tree_.getChain(kdl_tree_.getRootSegment()->first, this->robot_namespace_ + "_base_link", mount_chain);
//cout << "KDL segment 0 name: " << mount_chain.getSegment(0).getName() << endl;
base_link2robot_ = mount_chain.getSegment(0).getFrameToTip();
//cout << "base_link2robot transform: " << endl << base_link2robot_ << endl;
joint_names_.push_back( robot_namespace_ + std::string("_0_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_1_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_2_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_3_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_4_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_5_joint") );
joint_names_.push_back( robot_namespace_ + std::string("_6_joint") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_xx") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_yx") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_zx") );
cart_12_names_.push_back( robot_namespace_ + std::string("_pos_x") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_xy") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_yy") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_zy") );
cart_12_names_.push_back( robot_namespace_ + std::string("_pos_y") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_xz") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_yz") );
cart_12_names_.push_back( robot_namespace_ + std::string("_rot_zz") );
cart_12_names_.push_back( robot_namespace_ + std::string("_pos_z") );
cart_6_names_.push_back( robot_namespace_ + std::string("_X") );
cart_6_names_.push_back( robot_namespace_ + std::string("_Y") );
cart_6_names_.push_back( robot_namespace_ + std::string("_Z") );
cart_6_names_.push_back( robot_namespace_ + std::string("_A") );
cart_6_names_.push_back( robot_namespace_ + std::string("_B") );
cart_6_names_.push_back( robot_namespace_ + std::string("_C") );
// now get all handles, 12 for cart pos, 6 for stiff, 6 for damp, 6 for wrench, 6 for joints
for(int c = 0; c < 30; ++c)
{
if(c < 12)
cart_handles_.push_back(robot->getHandle(cart_12_names_.at(c)));
if(c > 11 && c < 18)
cart_handles_.push_back(robot->getHandle(cart_6_names_.at(c-12) + std::string("_stiffness")));
if(c > 17 && c < 24)
cart_handles_.push_back(robot->getHandle(cart_6_names_.at(c-18) + std::string("_damping")));
if(c > 23 && c < 30)
cart_handles_.push_back(robot->getHandle(cart_6_names_.at(c-24) + std::string("_wrench")));
}
for(int j = 0; j < 6; ++j)
{
joint_handles_.push_back(robot->getHandle(joint_names_.at(j)));
}
sub_pose_ = n.subscribe(n.resolveName("pose"), 1, &ITRCartesianImpedanceController::pose, this);
sub_pose_world_ = n.subscribe(n.resolveName("pose_base_link"), 1, &ITRCartesianImpedanceController::pose_base_link, this);
sub_gains_ = n.subscribe(n.resolveName("gains"), 1, &ITRCartesianImpedanceController::gains, this);
sub_addFT_ = n.subscribe(n.resolveName("wrench"), 1, &ITRCartesianImpedanceController::additionalFT, this);
srv_command_ = n.advertiseService("set_command", &ITRCartesianImpedanceController::command_cb, this);
//sub_ft_measures_ = n.subscribe(n.resolveName("ft_measures"), 1, &ITRCartesianImpedanceController::updateFT, this);
//pub_goal_ = n.advertise<geometry_msgs::PoseStamped>(n.resolveName("goal"),0);
pub_msr_pos_ = n.advertise<geometry_msgs::PoseStamped>(n.resolveName("measured_cartesian_pose"),0);
// Initial position
KDL::Rotation cur_R(cart_handles_.at(0).getPosition(),
cart_handles_.at(1).getPosition(),
cart_handles_.at(2).getPosition(),
cart_handles_.at(4).getPosition(),
cart_handles_.at(5).getPosition(),
cart_handles_.at(6).getPosition(),
cart_handles_.at(8).getPosition(),
cart_handles_.at(9).getPosition(),
cart_handles_.at(10).getPosition());
KDL::Vector cur_p(cart_handles_.at(3).getPosition(),
cart_handles_.at(7).getPosition(),
cart_handles_.at(11).getPosition());
KDL::Frame cur_T( cur_R, cur_p );
x_ref_ = cur_T;
x_des_ = cur_T;
x_set_ = cur_T;
// Initial Cartesian stiffness
KDL::Stiffness k( 800.0, 800.0, 800.0, 50.0, 50.0, 50.0 );
k_des_ = k;
// Initial force/torque measure
KDL::Wrench w(KDL::Vector(0.0, 0.0, 0.0), KDL::Vector(0.0, 0.0, 0.0));
f_des_ = w;
// Initial Cartesian damping
KDL::Stiffness d(0.7, 0.7, 0.7, 0.7, 0.7, 0.7);
d_des_ = d;
for (int i = 0; i < 3; ++i){
if ( !nh_.getParam("position_stiffness_gains", k_des_[i] ) ){
ROS_WARN("Position stiffness gain not set in yaml file, Using %f", k_des_[i]);
}
}
for (int i = 3; i < 6; ++i){
if ( !nh_.getParam("orientation_stiffness_gains", k_des_[i] ) ){
ROS_WARN("Orientation stiffness gain not set in yaml file, Using %f", k_des_[i]);
}
}
for (int i = 0; i < 6; ++i){
if ( !nh_.getParam("damping_gains", d_des_[i]) ){
ROS_WARN("Damping gain not set in yaml file, Using %f", d_des_[i]);
}
}
max_trans_speed_ = 0.1; // m/s
max_rot_speed_ = 0.5; // rad/s
// get params for speed limit
if ( !nh_.getParam("max_trans_speed", max_trans_speed_) ){
ROS_WARN("Max translation speed not set in yaml file, Using %f", max_trans_speed_);
}
if ( !nh_.getParam("max_rot_speed", max_rot_speed_) ){
ROS_WARN("Max rotation speed not set in yaml file, Using %f", max_rot_speed_);
}
std::vector<double> cur_T_FRI;
fromKDLtoFRI(x_des_, cur_T_FRI);
// set initial commands already here:
for(int c = 0; c < 30; ++c)
{
if(c < 12)
cart_handles_.at(c).setCommand(cur_T_FRI.at(c));
if(c > 11 && c < 18)
cart_handles_.at(c).setCommand(k_des_[c-12]);
if(c > 17 && c < 24)
cart_handles_.at(c).setCommand(d_des_[c-18]);
if(c > 23 && c < 30)
cart_handles_.at(c).setCommand(f_des_[c-24]);
}
cmd_flag_ = false;
return true;
}
