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; }