cob_cartesian_trajectories::cob_cartesian_trajectories() : as_(n, "moveCirc", boost::bind(&cob_cartesian_trajectories::moveCircActionCB, this, _1), false), as2_(n, "moveLin", boost::bind(&cob_cartesian_trajectories::moveLinActionCB, this, _1), false), as_model_(n, "moveModel", boost::bind(&cob_cartesian_trajectories::moveModelActionCB, this, _1), false)
{
ros::NodeHandle node;
node.param("cob_cartesian_trajectories_PID/p_gain", p_gain_, 1.0);
node.param("cob_cartesian_trajectories_PID/i_gain", i_gain_, 0.002);
node.param("cob_cartesian_trajectories_PID/d_gain", d_gain_, 0.0);
ROS_INFO("Starting PID controller with P: %e, I: %e, D: %e", p_gain_, i_gain_, d_gain_);
cart_state_sub_ = n.subscribe("/arm_controller/cart_state", 1, &cob_cartesian_trajectories::cartStateCallback, this);
joint_state_sub_ = n.subscribe("/joint_states", 1, &cob_cartesian_trajectories::jointStateCallback, this);
cart_command_pub = n.advertise<geometry_msgs::Twist>("/arm_controller/cart_command",1);
debug_cart_pub_ = n.advertise<geometry_msgs::PoseArray>("/mm/debug",1);
serv_prismatic = n.advertiseService("/mm/move_pri", &cob_cartesian_trajectories::movePriCB, this); // new service
serv_rotational = n.advertiseService("/mm/move_rot", &cob_cartesian_trajectories::moveRotCB, this); // new service
serv_model = n.advertiseService("/mm/move_model", &cob_cartesian_trajectories::moveModelCB, this); // new service to work with models
map_pub_ = n.advertise<visualization_msgs::Marker>("/visualization_marker", 1);
twist_pub_ = n.advertise<visualization_msgs::Marker>("/visualization_marker", 1); // publish twist to be visualized inj rviz
track_pub_ = n.advertise<articulation_msgs::TrackMsg>("/track", 1); // publish generated trajectory for debugging
bRun = false;
as_.start();
as2_.start();
as_model_.start();
targetDuration = 0;
currentDuration = 0;
mode = "prismatic"; // prismatic, rotational, trajectory, model
pub_timer = ros::Time::now();
pub_counter = 0;
double const PI = 4.0*std::atan(1.0);
getJointLimits(UpperLimits, LowerLimits); // get arm joint limits from topic /robot_description
}
bool KDLRobotModel::getJointLimits(std::vector<std::string> &joint_names, std::vector<double> &min_limits, std::vector<double> &max_limits, std::vector<bool> &continuous)
{
min_limits.resize(joint_names.size());
max_limits.resize(joint_names.size());
continuous.resize(joint_names.size());
for(size_t i = 0; i < joint_names.size(); ++i)
{
if(joint_names[i].empty())
{
ROS_ERROR("Empty joint name found.");
return false;
}
bool c;
if(!getJointLimits(joint_names[i], min_limits[i], max_limits[i], c))
{
ROS_ERROR("Joint limits were not found for %s.", joint_names[i].c_str());
return false;
}
continuous[i] = c;
}
return true;
}
bool KDLRobotModel::init(std::string robot_description, std::vector<std::string> &planning_joints)
{
urdf_ = boost::shared_ptr<urdf::Model>(new urdf::Model());
if (!urdf_->initString(robot_description))
{
ROS_ERROR("Failed to parse the URDF.");
return false;
}
if (!kdl_parser::treeFromUrdfModel(*urdf_, ktree_))
{
ROS_ERROR("Failed to parse the kdl tree from robot description.");
return false;
}
std::vector<std::string> segments(planning_joints.size());
for(size_t j = 0; j < planning_joints.size(); ++j)
{
if(!leatherman::getSegmentOfJoint(ktree_, planning_joints[j], segments[j]))
{
ROS_ERROR("Failed to find kdl segment for '%s'.", planning_joints_[j].c_str());
return false;
}
}
/*
if(!leatherman::getChainTip(ktree_, segments, chain_root_name_, chain_tip_name_))
{
ROS_ERROR("Failed to find a valid chain tip link.");
return false;
}
*/
if(!ktree_.getChain(chain_root_name_, chain_tip_name_, kchain_))
{
ROS_ERROR("Failed to fetch the KDL chain for the robot. (root: %s, tip: %s)", chain_root_name_.c_str(), chain_tip_name_.c_str());
return false;
}
// check if our chain includes all planning joints
for(size_t i = 0; i < planning_joints.size(); ++i)
{
if(planning_joints[i].empty())
{
ROS_ERROR("Planning joint name is empty (index: %d).", int(i));
return false;
}
int index;
if(!leatherman::getJointIndex(kchain_, planning_joints[i], index))
{
ROS_ERROR("Failed to find '%s' in the kinematic chain. Maybe your chain root or tip joints are wrong? (%s, %s)", planning_joints[i].c_str(), chain_root_name_.c_str(), chain_tip_name_.c_str());
return false;
}
}
// joint limits
planning_joints_ = planning_joints;
if(!getJointLimits(planning_joints_, min_limits_, max_limits_, continuous_))
{
ROS_ERROR("Failed to get the joint limits.");
return false;
}
// FK solver
fk_solver_ = new KDL::ChainFkSolverPos_recursive(kchain_);
jnt_pos_in_.resize(kchain_.getNrOfJoints());
jnt_pos_out_.resize(kchain_.getNrOfJoints());
// IK solver
KDL::JntArray q_min(planning_joints_.size());
KDL::JntArray q_max(planning_joints_.size());
for(size_t i = 0; i < planning_joints_.size(); ++i)
{
q_min(i) = min_limits_[i];
q_max(i) = max_limits_[i];
}
ik_vel_solver_ = new KDL::ChainIkSolverVel_pinv(kchain_);
ik_solver_ = new KDL::ChainIkSolverPos_NR_JL(kchain_, q_min, q_max, *fk_solver_, *ik_vel_solver_, 200, 0.001);
// joint name -> index mapping
for(size_t i = 0; i < planning_joints_.size(); ++i)
joint_map_[planning_joints_[i]] = i;
// link name -> kdl index mapping
for(size_t i = 0; i < kchain_.getNrOfSegments(); ++i)
link_map_[kchain_.getSegment(i).getName()] = i;
initialized_ = true;
return true;
}
