/*!
* \brief Publishes the current joint angles.
*
* Joint angles are published in both their raw state as obtained from the arm
* (JointAngles), and transformed & converted to radians (joint_state) as per
* the Kinova Kinematics PDF.
*
* Velocities and torques (effort) are only published in the JointStates
* message, only for the first 6 joints as these values are not available for
* the fingers.
*/
void KinovaArm::publishJointAngles(void)
{
FingerAngles fingers;
kinova_comm_.getFingerPositions(fingers);
if (arm_joint_number_ != 4 && arm_joint_number_ != 6 && arm_joint_number_ != 7)
{
ROS_WARN_ONCE("The joint_state publisher only supports 4, 6 and 7 DOF for now.: %d", arm_joint_number_);
}
// Query arm for current joint angles
KinovaAngles current_angles;
kinova_comm_.getJointAngles(current_angles);
kinova_msgs::JointAngles kinova_angles = current_angles.constructAnglesMsg();
AngularPosition joint_command;
kinova_comm_.getAngularCommand(joint_command);
kinova_msgs::JointAngles joint_command_msg = KinovaAngles(joint_command.Actuators).constructAnglesMsg();
sensor_msgs::JointState joint_state;
joint_state.name = joint_names_;
joint_state.header.stamp = ros::Time::now();
// Transform from Kinova DH algorithm to physical angles in radians, then place into vector array
joint_state.position.resize(joint_total_number_);
joint_state.position[0] = kinova_angles.joint1 * M_PI/180;
joint_state.position[1] = kinova_angles.joint2 * M_PI/180;
joint_state.position[2] = kinova_angles.joint3 * M_PI/180;
joint_state.position[3] = kinova_angles.joint4 * M_PI/180;
if (arm_joint_number_ >= 6)
{
joint_state.position[4] = kinova_angles.joint5 * M_PI/180;
joint_state.position[5] = kinova_angles.joint6 * M_PI/180;
}
if (arm_joint_number_ == 7)
{
joint_state.position[6] = kinova_angles.joint7 * M_PI/180;
}
if(finger_number_==2)
{
joint_state.position[joint_total_number_-2] = fingers.Finger1/6800*80*M_PI/180;
joint_state.position[joint_total_number_-1] = fingers.Finger2/6800*80*M_PI/180;
}
else if(finger_number_==3)
{
joint_state.position[joint_total_number_-3] = fingers.Finger1/6800*80*M_PI/180;
joint_state.position[joint_total_number_-2] = fingers.Finger2/6800*80*M_PI/180;
joint_state.position[joint_total_number_-1] = fingers.Finger3/6800*80*M_PI/180;
}
// Joint velocities
KinovaAngles current_vels;
kinova_comm_.getJointVelocities(current_vels);
joint_state.velocity.resize(joint_total_number_);
joint_state.velocity[0] = current_vels.Actuator1;
joint_state.velocity[1] = current_vels.Actuator2;
joint_state.velocity[2] = current_vels.Actuator3;
joint_state.velocity[3] = current_vels.Actuator4;
// no velocity info for fingers
if(finger_number_==2)
{
joint_state.velocity[joint_total_number_-2] = 0;
joint_state.velocity[joint_total_number_-1] = 0;
}
else if(finger_number_==3)
{
joint_state.velocity[joint_total_number_-3] = 0;
joint_state.velocity[joint_total_number_-2] = 0;
joint_state.velocity[joint_total_number_-1] = 0;
}
if (arm_joint_number_ >= 6)
{
joint_state.velocity[4] = current_vels.Actuator5;
joint_state.velocity[5] = current_vels.Actuator6;
}
if (arm_joint_number_ == 7)
{
joint_state.velocity[6] = current_vels.Actuator7;
}
// ROS_DEBUG_THROTTLE(0.1,
// "Raw joint velocities: %f %f %f %f %f %f",
// current_vels.Actuator1,
// current_vels.Actuator2,
// current_vels.Actuator3,
// current_vels.Actuator4,
// current_vels.Actuator5,
// current_vels.Actuator6);
if (convert_joint_velocities_) {
convertKinDeg(joint_state.velocity);
}
// Joint torques (effort)
KinovaAngles joint_tqs;
bool gravity_comp;
node_handle_.param("torque_parameters/publish_torque_with_gravity_compensation", gravity_comp, false);
if (gravity_comp==true)
kinova_comm_.getGravityCompensatedTorques(joint_tqs);
else
kinova_comm_.getJointTorques(joint_tqs);
joint_torque_publisher_.publish(joint_tqs.constructAnglesMsg());
joint_state.effort.resize(joint_total_number_);
joint_state.effort[0] = joint_tqs.Actuator1;
joint_state.effort[1] = joint_tqs.Actuator2;
joint_state.effort[2] = joint_tqs.Actuator3;
joint_state.effort[3] = joint_tqs.Actuator4;
// no effort info for fingers
if(finger_number_==2)
{
joint_state.effort[joint_total_number_-2] = 0;
joint_state.effort[joint_total_number_-1] = 0;
}
else if(finger_number_==3)
{
joint_state.effort[joint_total_number_-3] = 0;
joint_state.effort[joint_total_number_-2] = 0;
joint_state.effort[joint_total_number_-1] = 0;
}
if (arm_joint_number_ >= 6)
{
joint_state.effort[4] = joint_tqs.Actuator5;
joint_state.effort[5] = joint_tqs.Actuator6;
}
if (arm_joint_number_ == 7)
{
joint_state.effort[6] = joint_tqs.Actuator7;
}
joint_angles_publisher_.publish(kinova_angles);
joint_command_publisher_.publish(joint_command_msg);
joint_state_publisher_.publish(joint_state);
}
/*!
* \brief Publishes the current joint angles.
*
* Joint angles are published in both their raw state as obtained from the arm
* (JointAngles), and transformed & converted to radians (joint_state) as per
* the Kinova Kinematics PDF.
*
* Velocities and torques (effort) are only published in the JointStates
* message, only for the first 6 joints as these values are not available for
* the fingers.
*/
void KinovaArm::publishJointAngles(void)
{
FingerAngles fingers;
kinova_comm_.getFingerPositions(fingers);
if (arm_joint_number_ != 4 && arm_joint_number_ != 6)
{
ROS_WARN_ONCE("The joint_state publisher only supports 4DOF and 6DOF for now.: %d", arm_joint_number_);
}
// Query arm for current joint angles
KinovaAngles current_angles;
kinova_comm_.getJointAngles(current_angles);
kinova_msgs::JointAngles kinova_angles = current_angles.constructAnglesMsg();
AngularPosition joint_command;
kinova_comm_.getAngularCommand(joint_command);
kinova_msgs::JointAngles joint_command_msg = KinovaAngles(joint_command.Actuators).constructAnglesMsg();
sensor_msgs::JointState joint_state;
joint_state.name = joint_names_;
joint_state.header.stamp = ros::Time::now();
// Transform from Kinova DH algorithm to physical angles in radians, then place into vector array
joint_state.position.resize(joint_total_number_);
joint_state.position[0] = kinova_angles.joint1 * M_PI/180;
joint_state.position[1] = kinova_angles.joint2 * M_PI/180;
joint_state.position[2] = kinova_angles.joint3 * M_PI/180;
joint_state.position[3] = kinova_angles.joint4 * M_PI/180;
if (arm_joint_number_ == 6)
{
joint_state.position[4] = kinova_angles.joint5 * M_PI/180;
joint_state.position[5] = kinova_angles.joint6 * M_PI/180;
}
if(finger_number_==2)
{
joint_state.position[joint_total_number_-2] = 0;
joint_state.position[joint_total_number_-1] = 0;
}
else if(finger_number_==3)
{
joint_state.position[joint_total_number_-3] = 0;
joint_state.position[joint_total_number_-2] = 0;
joint_state.position[joint_total_number_-1] = 0;
}
// Joint velocities
KinovaAngles current_vels;
kinova_comm_.getJointVelocities(current_vels);
joint_state.velocity.resize(joint_total_number_);
joint_state.velocity[0] = current_vels.Actuator1;
joint_state.velocity[1] = current_vels.Actuator2;
joint_state.velocity[2] = current_vels.Actuator3;
joint_state.velocity[3] = current_vels.Actuator4;
// no velocity info for fingers
if(finger_number_==2)
{
joint_state.velocity[joint_total_number_-2] = 0;
joint_state.velocity[joint_total_number_-1] = 0;
}
else if(finger_number_==3)
{
joint_state.velocity[joint_total_number_-3] = 0;
joint_state.velocity[joint_total_number_-2] = 0;
joint_state.velocity[joint_total_number_-1] = 0;
}
if (arm_joint_number_ == 6)
{
joint_state.velocity[4] = current_vels.Actuator5;
joint_state.velocity[5] = current_vels.Actuator6;
}
// ROS_DEBUG_THROTTLE(0.1,
// "Raw joint velocities: %f %f %f %f %f %f",
// current_vels.Actuator1,
// current_vels.Actuator2,
// current_vels.Actuator3,
// current_vels.Actuator4,
// current_vels.Actuator5,
// current_vels.Actuator6);
if (convert_joint_velocities_) {
convertKinDeg(joint_state.velocity);
}
// Joint torques (effort)
// NOTE: Currently invalid.
KinovaAngles joint_tqs;
joint_state.effort.resize(joint_total_number_);
joint_state.effort[0] = joint_tqs.Actuator1;
joint_state.effort[1] = joint_tqs.Actuator2;
joint_state.effort[2] = joint_tqs.Actuator3;
joint_state.effort[3] = joint_tqs.Actuator4;
// no effort info for fingers
if(finger_number_==2)
{
joint_state.effort[joint_total_number_-2] = 0;
joint_state.effort[joint_total_number_-1] = 0;
}
else if(finger_number_==3)
{
joint_state.effort[joint_total_number_-3] = 0;
joint_state.effort[joint_total_number_-2] = 0;
joint_state.effort[joint_total_number_-1] = 0;
}
if (arm_joint_number_ == 6)
{
joint_state.effort[4] = joint_tqs.Actuator5;
joint_state.effort[5] = joint_tqs.Actuator6;
}
joint_angles_publisher_.publish(kinova_angles);
joint_command_publisher_.publish(joint_command_msg);
joint_state_publisher_.publish(joint_state);
}
