/*!
* \brief Publishes the current cartesian forces at the end effector.
*/
void JacoArm::publishToolWrench(void)
{
JacoPose wrench;
geometry_msgs::WrenchStamped current_wrench;
jaco_comm_.getCartesianForce(wrench);
current_wrench.wrench = wrench.constructWrenchMsg();
current_wrench.header.stamp = ros::Time::now();
// TODO: Rotate wrench to fit the end effector frame.
// Right now, the orientation of the wrench is in the API's (base) frame.
current_wrench.header.frame_id = tf_prefix_ + "api_origin";
// Same conversion issue as with velocities:
if (convert_joint_velocities_) {
convertKinDeg(current_wrench.wrench.torque);
}
tool_wrench_publisher_.publish(current_wrench);
}
/*!
* \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 Jaco 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 JacoArm::publishJointAngles(void)
{
FingerAngles fingers;
jaco_comm_.getFingerPositions(fingers);
// Query arm for current joint angles
JacoAngles current_angles;
jaco_comm_.getJointAngles(current_angles);
jaco_msgs::JointAngles jaco_angles = current_angles.constructAnglesMsg();
sensor_msgs::JointState joint_state;
joint_state.name = joint_names_;
// joint_state.header.stamp = ros::Time::now();
sensor_msgs::JointState joint_position_state;
joint_position_state.name = joint_urdf_names_;
joint_position_state.header.stamp = ros::Time::now();
//jaco_angles.joint1 = current_angles.Actuator1;
//jaco_angles.joint2 = current_angles.Actuator2;
//jaco_angles.joint3 = current_angles.Actuator3;
//jaco_angles.joint4 = current_angles.Actuator4;
//jaco_angles.joint5 = current_angles.Actuator5;
//jaco_angles.joint6 = current_angles.Actuator6;
// Transform from physical angles to Kinova DH algorithm in radians , then place into vector array
joint_state.position.resize(9);
joint_state.position[0] = jaco_angles.joint1;
joint_state.position[1] = jaco_angles.joint2;
joint_state.position[2] = jaco_angles.joint3;
joint_state.position[3] = jaco_angles.joint4;
joint_state.position[4] = jaco_angles.joint5;
joint_state.position[5] = jaco_angles.joint6;
joint_state.position[6] = finger_conv_ratio_ * fingers.Finger1;
joint_state.position[7] = finger_conv_ratio_ * fingers.Finger2;
joint_state.position[8] = finger_conv_ratio_ * fingers.Finger3;
// just for visualization
joint_position_state.position.resize(6);
joint_position_state.position[0] = jaco_angles.joint1;
joint_position_state.position[1] = jaco_angles.joint2;
joint_position_state.position[2] = jaco_angles.joint3;
joint_position_state.position[3] = jaco_angles.joint4;
joint_position_state.position[4] = jaco_angles.joint5;
joint_position_state.position[5] = jaco_angles.joint6;
// Joint velocities
JacoAngles current_vels;
jaco_comm_.getJointVelocities(current_vels);
joint_state.velocity.resize(9);
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;
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",
joint_state.velocity[0],
joint_state.velocity[1],
joint_state.velocity[2],
joint_state.velocity[3],
joint_state.velocity[4],
joint_state.velocity[5]);
if (convert_joint_velocities_) {
convertKinDeg(joint_state.velocity);
}
// No velocity for the fingers:
joint_state.velocity[6] = 0.0;
joint_state.velocity[7] = 0.0;
joint_state.velocity[8] = 0.0;
joint_position_state.velocity.resize(6);
joint_position_state.velocity[0] = current_vels.Actuator1;
joint_position_state.velocity[1] = current_vels.Actuator2;
joint_position_state.velocity[2] = current_vels.Actuator3;
joint_position_state.velocity[3] = current_vels.Actuator4;
joint_position_state.velocity[4] = current_vels.Actuator5;
joint_position_state.velocity[5] = current_vels.Actuator6;
// Joint torques (effort)
// NOTE: Currently invalid.
JacoAngles joint_tqs;
joint_state.effort.resize(9);
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;
joint_state.effort[4] = joint_tqs.Actuator5;
joint_state.effort[5] = joint_tqs.Actuator6;
joint_state.effort[6] = 0.0;
joint_state.effort[7] = 0.0;
joint_state.effort[8] = 0.0;
ROS_DEBUG_THROTTLE(0.1,
"Raw joint torques: %f %f %f %f %f %f",
joint_state.effort[0],
joint_state.effort[1],
joint_state.effort[2],
joint_state.effort[3],
joint_state.effort[4],
joint_state.effort[5]);
joint_position_state.effort.resize(6);
joint_position_state.effort[0] = joint_tqs.Actuator1;
joint_position_state.effort[1] = joint_tqs.Actuator2;
joint_position_state.effort[2] = joint_tqs.Actuator3;
joint_position_state.effort[3] = joint_tqs.Actuator4;
joint_position_state.effort[4] = joint_tqs.Actuator5;
joint_position_state.effort[5] = joint_tqs.Actuator6;
joint_angles_publisher_.publish(jaco_angles);
joint_state_publisher_.publish(joint_state);
joint_position_state_publisher_.publish(joint_position_state);
ros::spinOnce();
}
/*!
* \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);
}
/*!
* \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 Jaco 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 JacoArm::publishJointAngles(void)
{
FingerAngles fingers;
jaco_comm_.getFingerPositions(fingers);
// Query arm for current joint angles
JacoAngles current_angles;
jaco_comm_.getJointAngles(current_angles);
jaco_msgs::JointAngles jaco_angles = current_angles.constructAnglesMsg(robot_type);
jaco_angles.joint1 = current_angles.Actuator1;
jaco_angles.joint2 = current_angles.Actuator2;
jaco_angles.joint3 = current_angles.Actuator3;
jaco_angles.joint4 = current_angles.Actuator4;
jaco_angles.joint5 = current_angles.Actuator5;
jaco_angles.joint6 = current_angles.Actuator6;
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(jaco_joints_count);
double j6o = robot_type == ROBOT_TYPE_JACO ? JACO_JOINT_2_ANGLE : MICO_JOINT_2_ANGLE;
joint_state.position[0] = (180- jaco_angles.joint1) * (PI / 180);
joint_state.position[1] = (jaco_angles.joint2 - j6o) * (PI / 180);
joint_state.position[2] = (90-jaco_angles.joint3) * (PI / 180);
joint_state.position[3] = (180-jaco_angles.joint4) * (PI / 180);
joint_state.position[4] = (180-jaco_angles.joint5) * (PI / 180);
joint_state.position[5] = (j6o-jaco_angles.joint6) * (PI / 180);
if (joint_state.position[5] > PI) {
joint_state.position[5] -= (2*PI);
}
joint_state.position[6] = finger_conv_ratio_ * fingers.Finger1;
joint_state.position[7] = finger_conv_ratio_ * fingers.Finger2;
if(robot_type == ROBOT_TYPE_JACO) {
joint_state.position[8] = finger_conv_ratio_ * fingers.Finger3;
}
// Joint velocities
JacoAngles current_vels;
jaco_comm_.getJointVelocities(current_vels);
joint_state.velocity.resize(jaco_joints_count);
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;
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",
joint_state.velocity[0],
joint_state.velocity[1],
joint_state.velocity[2],
joint_state.velocity[3],
joint_state.velocity[4],
joint_state.velocity[5]);
if (convert_joint_velocities_) {
convertKinDeg(joint_state.velocity);
}
// No velocity for the fingers:
joint_state.velocity[6] = 0.0;
joint_state.velocity[7] = 0.0;
if(robot_type==ROBOT_TYPE_JACO){
joint_state.velocity[8] = 0.0;
}
// Joint torques (effort)
// NOTE: Currently invalid.
JacoAngles joint_tqs;
joint_state.effort.resize(jaco_joints_count);
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;
joint_state.effort[4] = joint_tqs.Actuator5;
joint_state.effort[5] = joint_tqs.Actuator6;
joint_state.effort[6] = 0.0;
joint_state.effort[7] = 0.0;
if(robot_type==ROBOT_TYPE_JACO){
joint_state.effort[8] = 0.0;
}
ROS_DEBUG_THROTTLE(0.1,
"Raw joint torques: %f %f %f %f %f %f",
joint_state.effort[0],
joint_state.effort[1],
joint_state.effort[2],
joint_state.effort[3],
joint_state.effort[4],
joint_state.effort[5]);
joint_angles_publisher_.publish(jaco_angles);
joint_state_publisher_.publish(joint_state);
}
