/*! * \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); }