Error using KinovaComm::setCartesianPosition() about kinova-ros

[felixmaisonneuve]

Hi @NAEE09,

The error you get is printed here :

kinova-ros/kinova_driver/src/kinova_comm.cpp

Lines 1389 to 1407 in 93f6682

	void KinovaComm::getFingerPositions(FingerAngles &fingers)
	{
	boost::recursive_mutex::scoped_lock lock(api_mutex_);
	CartesianPosition kinova_cartesian_position;
	memset(&kinova_cartesian_position, 0, sizeof(kinova_cartesian_position)); // zero structure
	int result = kinova_api_.getCartesianPosition(kinova_cartesian_position);
	if (result != NO_ERROR_KINOVA)
	{
	throw KinovaCommException("Could not get Cartesian finger position", result);
	}
	if (num_fingers_ == 2)
	{
	kinova_cartesian_position.Fingers.Finger3 = 0.0;
	}
	fingers = FingerAngles(kinova_cartesian_position.Fingers);
	}

Since you do not have a gripper, this API call will always fail, so you have to make sure this is never called in your driver.

The kinova_arm driver constantly ppublishes info about the arm, and I think this is were it fails.

This is the function in the driver that updates the topics of your ROS driver :

kinova-ros/kinova_driver/src/kinova_arm.cpp

Lines 759 to 765 in 93f6682

	void KinovaArm::statusTimer(const ros::TimerEvent&)
	{
	publishJointAngles();
	publishToolPosition();
	publishToolWrench();
	publishFingerPosition();
	}

You need to remove the publishFingerPosition() function (that uses getFingerPositions) and you need to modify the publishJointAngles() function also.

In publishJointAngles(), remove every mention to fingers :

kinova-ros/kinova_driver/src/kinova_arm.cpp

Lines 571 to 572 in 93f6682

	FingerAngles fingers;
	kinova_comm_.getFingerPositions(fingers);

kinova-ros/kinova_driver/src/kinova_arm.cpp

Lines 608 to 627 in 93f6682

	if(finger_number_==2)
	{
	// proximal phalanges
	joint_state.position[joint_total_number_-4] = fingers.Finger1 * finger_conv_ratio_ * M_PI/180;
	joint_state.position[joint_total_number_-3] = fingers.Finger2 * finger_conv_ratio_ * M_PI/180;
	// distal phalanges
	joint_state.position[joint_total_number_-2] = 0;
	joint_state.position[joint_total_number_-1] = 0;
	}
	else if(finger_number_==3)
	{
	// proximal phalanges
	joint_state.position[joint_total_number_-6] = fingers.Finger1 * finger_conv_ratio_ * M_PI/180;
	joint_state.position[joint_total_number_-5] = fingers.Finger2 * finger_conv_ratio_ * M_PI/180;
	joint_state.position[joint_total_number_-4] = fingers.Finger3 * finger_conv_ratio_ * M_PI/180;
	// distal phalanges
	joint_state.position[joint_total_number_-3] = 0;
	joint_state.position[joint_total_number_-2] = 0;
	joint_state.position[joint_total_number_-1] = 0;
	}

If you remove all of this, your driver should be able to launch correctly.

For the kinovaComm::setCartesianPosition(), can you share a brief sample of your code? I am not sure I fully understand what you are doing

Regards,
Felix