KinovaArm::KinovaArm(KinovaComm &arm, const ros::NodeHandle &nodeHandle, const std::string &kinova_robotType, const std::string &kinova_robotName)
: kinova_comm_(arm), node_handle_(nodeHandle), kinova_robotType_(kinova_robotType), kinova_robotName_(kinova_robotName)
{
for (int i=0;i<COMMAND_SIZE;i++)
{
l_joint_torque_[i] = 0;
l_force_cmd_[i] = 0;
}
//multiple arms
if (node_handle_.hasParam("/kinova_robots"))
{
XmlRpc::XmlRpcValue robot_list;
node_handle_.getParam("/kinova_robots", robot_list);
if (robot_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("Parameter controller_list should be specified as an array");
return;
}
robots_.resize(robot_list.size());
for (int i = 0; i < robot_list.size(); ++i)
{
if (!robot_list[i].hasMember("name") || !robot_list[i].hasMember("serial"))
{
ROS_ERROR_STREAM("Name and serial must be specifed for each robot");
continue;
}
robots_[i].name = std::string(robot_list[i]["name"]);
robots_[i].name = std::string(robot_list[i]["type"]);
robots_[i].name = std::string(robot_list[i]["serial"]);
}
}
/* Set up parameters for different robot type */
// example for a kinova_robotType: j2n6s300
if (valid_kinovaRobotType(kinova_robotType_) == false)
{
ROS_WARN("Invalid kinova_robotType error! Obtained: %s.", kinova_robotType_.c_str());
return;
}
// tf_prefix_ = kinova_robotType_ + "_" + boost::lexical_cast<string>(same_type_index); // in case of multiple same_type robots
tf_prefix_ = kinova_robotName_ + "_";
// Maximum number of joints on Kinova-like robots:
robot_category_ = kinova_robotType_[0];
robot_category_version_ = kinova_robotType_[1]-'0';
wrist_type_ = kinova_robotType_[2];
arm_joint_number_ = kinova_robotType_[3]-'0';
robot_mode_ = kinova_robotType_[4];
finger_number_ = kinova_robotType_[5]-'0';
joint_total_number_ = arm_joint_number_ + finger_number_;
if (robot_category_=='j') // jaco robot
{
// special parameters for jaco
if (arm_joint_number_ == 6)
{
if (wrist_type_ == 'n')
robot_type_ = JACOV2_6DOF_SERVICE;
else
robot_type_ = SPHERICAL_6DOF_SERVICE;
}
else if (arm_joint_number_ == 4)
{
robot_type_ = JACOV2_4DOF_SERVICE;
}
else if (arm_joint_number_ == 7)
{
robot_type_ = SPHERICAL_7DOF_SERVICE;
}
}
else if (robot_category_ == 'm') // mico robot
{
// special parameters for mico
if (arm_joint_number_ == 6)
robot_type_ = MICO_6DOF_SERVICE;
else if (arm_joint_number_ == 4)
robot_type_ = MICO_4DOF_SERVICE;
}
else if (robot_category_ == 'r') // roco robot
{
// special parameters for roco
}
else
{
// special parameters for custom robot or other cases
}
if (finger_number_==3)
{
// finger_angle_conv_ratio used to display finger position properly in Rviz
// Approximative conversion ratio from finger position (0..6400) to joint angle
// in radians (0.. 1.4) for 3 finger hand
finger_conv_ratio_= 1.4 / 6400.0;
}
else if(finger_number_==2)
{
// the two finger hand may different
finger_conv_ratio_= 1.4 / 6400.0;
}
else
{
// no fingers
}
for (int i = 0; i<arm_joint_number_; i++)
{
joint_names_.resize(joint_total_number_);
joint_names_[i] = tf_prefix_ + "joint_" + boost::lexical_cast<std::string>(i+1);
}
for (int i = 0; i<finger_number_; i++)
{
joint_names_[arm_joint_number_+i] = tf_prefix_ + "joint_finger_" + boost::lexical_cast<std::string>(i+1);
}
/* Set up Services */
stop_service_ = node_handle_.advertiseService("in/stop", &KinovaArm::stopServiceCallback, this);
start_service_ = node_handle_.advertiseService("in/start", &KinovaArm::startServiceCallback, this);
homing_service_ = node_handle_.advertiseService("in/home_arm", &KinovaArm::homeArmServiceCallback, this);
add_trajectory_ = node_handle_.advertiseService("in/add_pose_to_Cartesian_trajectory",
&KinovaArm::addCartesianPoseToTrajectory, this);
clear_trajectories_ = node_handle_.advertiseService("in/clear_trajectories",
&KinovaArm::clearTrajectoriesServiceCallback, this);
set_force_control_params_service_ = node_handle_.advertiseService("in/set_force_control_params", &KinovaArm::setForceControlParamsCallback, this);
start_force_control_service_ = node_handle_.advertiseService("in/start_force_control", &KinovaArm::startForceControlCallback, this);
stop_force_control_service_ = node_handle_.advertiseService("in/stop_force_control", &KinovaArm::stopForceControlCallback, this);
set_actuator_torques_to_zero_ = node_handle_.advertiseService(
"in/set_zero_torques", &KinovaArm::setJointTorquesToZeroService, this);
run_COM_parameter_estimation_service_ = node_handle_.advertiseService(
"in/run_COM_parameters_estimation",
&KinovaArm::runCOMParameterEstimationService,this);
set_end_effector_offset_service_ = node_handle_.advertiseService("in/set_end_effector_offset",
&KinovaArm::setEndEffectorOffsetCallback, this);
start_null_space_service_ = node_handle_.advertiseService("in/set_null_space_mode_state", &KinovaArm::ActivateNullSpaceModeCallback, this);
set_torque_control_mode_service_ = node_handle_.advertiseService("in/set_torque_control_mode", &KinovaArm::setTorqueControlModeService, this);
set_torque_control_parameters_service_ = node_handle_.advertiseService
("in/set_torque_control_parameters",
&KinovaArm::setTorqueControlParametersService,this);
/* Set up Publishers */
joint_angles_publisher_ = node_handle_.advertise<kinova_msgs::JointAngles>
("out/joint_angles", 2);
joint_torque_publisher_ = node_handle_.advertise<kinova_msgs::JointAngles>
("out/joint_torques", 2);
joint_state_publisher_ = node_handle_.advertise<sensor_msgs::JointState>
("out/joint_state", 2);
tool_position_publisher_ = node_handle_.advertise<geometry_msgs::PoseStamped>
("out/tool_pose", 2);
tool_wrench_publisher_ = node_handle_.advertise<geometry_msgs::WrenchStamped>
("out/tool_wrench", 2);
finger_position_publisher_ = node_handle_.advertise<kinova_msgs::FingerPosition>
("out/finger_position", 2);
// Publish last command for relative motion (read current position cause arm drop)
joint_command_publisher_ = node_handle_.advertise<kinova_msgs::JointAngles>("out/joint_command", 2);
cartesian_command_publisher_ = node_handle_.advertise<kinova_msgs::KinovaPose>("out/cartesian_command", 2);
/* Set up Subscribers*/
joint_velocity_subscriber_ = node_handle_.subscribe("in/joint_velocity", 1,
&KinovaArm::jointVelocityCallback, this);
cartesian_velocity_subscriber_ = node_handle_.subscribe("in/cartesian_velocity", 1,
&KinovaArm::cartesianVelocityCallback, this);
joint_torque_subscriber_ = node_handle_.subscribe("in/joint_torque", 1,
&KinovaArm::jointTorqueSubscriberCallback, this);
cartesian_force_subscriber_ = node_handle_.subscribe("in/cartesian_force", 1,
&KinovaArm::forceSubscriberCallback, this);
node_handle_.param<double>("status_interval_seconds", status_interval_seconds_, 0.1);
// Depending on the API version, the arm might return velocities in the
// 0..360 range (0..180 for positive values, 181..360 for negative ones).
// This indicates that the ROS node should convert them first before
// updating the joint_state topic.
node_handle_.param("convert_joint_velocities", convert_joint_velocities_, true);
status_timer_ = node_handle_.createTimer(ros::Duration(status_interval_seconds_),
&KinovaArm::statusTimer, this);
ROS_INFO("The arm is ready to use.");
}
KinovaArm::KinovaArm(KinovaComm &arm, const ros::NodeHandle &nodeHandle, const std::string &kinova_robotType)
: kinova_comm_(arm), node_handle_(nodeHandle), kinova_robotType_(kinova_robotType)
{
/* Set up parameters for different robot type */
// example for a kinova_robotType: j2n6s300
if (valid_kinovaRobotType(kinova_robotType_) == false)
{
ROS_WARN("Invalid kinova_robotType error! Obtained: %s.", kinova_robotType_.c_str());
return;
}
// tf_prefix_ = kinova_robotType_ + "_" + boost::lexical_cast<string>(same_type_index); // in case of multiple same_type robots
tf_prefix_ = kinova_robotType_ + "_";
// Maximum number of joints on Kinova-like robots:
robot_category_ = kinova_robotType_[0];
robot_category_version_ = kinova_robotType_[1]-'0';
wrist_type_ = kinova_robotType_[2];
arm_joint_number_ = kinova_robotType_[3]-'0';
robot_mode_ = kinova_robotType_[4];
finger_number_ = kinova_robotType_[5]-'0';
joint_total_number_ = arm_joint_number_ + finger_number_;
if (robot_category_=='j') // jaco robot
{
// special parameters for jaco
}
else if (robot_category_ == 'm') // mico robot
{
// special parameters for mico
}
else if (robot_category_ == 'r') // roco robot
{
// special parameters for roco
}
else
{
// special parameters for custom robot or other cases
}
if (finger_number_==3)
{
// finger_angle_conv_ratio used to display finger position properly in Rviz
// Approximative conversion ratio from finger position (0..6400) to joint angle
// in radians (0.. 1.4) for 3 finger hand
finger_conv_ratio_= 1.4 / 6400.0;
}
else if(finger_number_==2)
{
// the two finger hand may different
finger_conv_ratio_= 1.4 / 6400.0;
}
else
{
// no fingers
}
for (int i = 0; i<arm_joint_number_; i++)
{
joint_names_.resize(joint_total_number_);
joint_names_[i] = tf_prefix_ + "joint_" + boost::lexical_cast<std::string>(i+1);
}
for (int i = 0; i<finger_number_; i++)
{
joint_names_[arm_joint_number_+i] = tf_prefix_ + "joint_finger_" + boost::lexical_cast<std::string>(i+1);
}
/* Set up Services */
stop_service_ = node_handle_.advertiseService("in/stop", &KinovaArm::stopServiceCallback, this);
start_service_ = node_handle_.advertiseService("in/start", &KinovaArm::startServiceCallback, this);
homing_service_ = node_handle_.advertiseService("in/home_arm", &KinovaArm::homeArmServiceCallback, this);
set_force_control_params_service_ = node_handle_.advertiseService("in/set_force_control_params", &KinovaArm::setForceControlParamsCallback, this);
start_force_control_service_ = node_handle_.advertiseService("in/start_force_control", &KinovaArm::startForceControlCallback, this);
stop_force_control_service_ = node_handle_.advertiseService("in/stop_force_control", &KinovaArm::stopForceControlCallback, this);
set_end_effector_offset_service_ = node_handle_.advertiseService("in/set_end_effector_offset",
&KinovaArm::setEndEffectorOffsetCallback, this);
/* Set up Publishers */
joint_angles_publisher_ = node_handle_.advertise<kinova_msgs::JointAngles>("out/joint_angles", 2);
joint_state_publisher_ = node_handle_.advertise<sensor_msgs::JointState>("out/joint_state", 2);
tool_position_publisher_ = node_handle_.advertise<geometry_msgs::PoseStamped>("out/tool_pose", 2);
tool_wrench_publisher_ = node_handle_.advertise<geometry_msgs::WrenchStamped>("out/tool_wrench", 2);
finger_position_publisher_ = node_handle_.advertise<kinova_msgs::FingerPosition>("out/finger_position", 2);
// Publish last command for relative motion (read current position cause arm drop)
joint_command_publisher_ = node_handle_.advertise<kinova_msgs::JointAngles>("out/joint_command", 2);
cartesian_command_publisher_ = node_handle_.advertise<kinova_msgs::KinovaPose>("out/cartesian_command", 2);
/* Set up Subscribers*/
joint_velocity_subscriber_ = node_handle_.subscribe("in/joint_velocity", 1,
&KinovaArm::jointVelocityCallback, this);
cartesian_velocity_subscriber_ = node_handle_.subscribe("in/cartesian_velocity", 1,
&KinovaArm::cartesianVelocityCallback, this);
node_handle_.param<double>("status_interval_seconds", status_interval_seconds_, 0.1);
// Depending on the API version, the arm might return velocities in the
// 0..360 range (0..180 for positive values, 181..360 for negative ones).
// This indicates that the ROS node should convert them first before
// updating the joint_state topic.
node_handle_.param("convert_joint_velocities", convert_joint_velocities_, true);
status_timer_ = node_handle_.createTimer(ros::Duration(status_interval_seconds_),
&KinovaArm::statusTimer, this);
ROS_INFO("The arm is ready to use.");
}
