Can only perform kinematic computation for a single link (simple question)
I made an arm_navigation stack using the Planning Description Configuration Wizard. However, when I try to run forward kinematics on my robot, only a single link shows up in the GetKinematicSolverInfo service response.
[ INFO] [1398420068.098256875]: Joint: 0 shoulder_pan_joint
[ INFO] [1398420068.098394147]: Joint: 1 shoulder_lift_joint
[ INFO] [1398420068.098505827]: Joint: 2 elbow_joint
[ INFO] [1398420068.098577658]: Joint: 3 wrist_1_joint
[ INFO] [1398420068.098646898]: Joint: 4 wrist_2_joint
[ INFO] [1398420068.098723696]: Joint: 5 wrist_3_joint
[ INFO] [1398420068.098798122]:
[ INFO] [1398420068.098873428]: Link: 0 wrist_3_link
Did I make a mistake in the Planning Configuration Wizard, or must I look at the setup for my planner? There doesn't seem to be much precise information in the relevant tutorials.
EDIT: It could be that wrist_3_link is the only link that the solver will take as input, but if I try to solve for the forward kinematics for wrist_3_link, the kinematics server crasher. I assumed it was because I haven't specified the all the links, but that may not be the case. Perhaps there is a bug in the code, although I cannot find it.
#include <ros/ros.h>
#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/GetPositionFK.h>
class ur5_listener_class
{
private:
sensor_msgs::JointState ur5_state;
public:
void returnJointState(const sensor_msgs::JointState msg)
{
// The callback function only passes on the data to the class
ur5_state = msg;
}
sensor_msgs::JointState applyJointState()
{
// if the joint_state data is needed in the main function, it will be returned here.
return ur5_state;
}
void debug()
{
for(int i=0;i<6;i++)
{
std::cout << "The " << ur5_state.name.at(i) << " is now at " << ur5_state.position.at(i);
std::cout << " moving with velocity " << ur5_state.velocity.at(i) << "\n";
}
}
};
int main(int argc, char **argv){
// Variable declaration
sensor_msgs::JointState ur5_state;
ros::init (argc, argv, "get_fk");
ros::NodeHandle rh;
// Before other business, connect to node, and retrieve joint state
ros::Subscriber sub;
ur5_listener_class ur5_listener;
sub = rh.subscribe("/joint_states", 1000, &ur5_listener_class::returnJointState, &ur5_listener);
// Wait for the connection to establish
ros::Rate r(10);
while (sub.getNumPublishers() == 0)
r.sleep();
// retrieve joint_state and use for JointGoal
ros::spinOnce();
ur5_state = ur5_listener.applyJointState();
ros::service::waitForService("ur5_UR5_chain_kinematics/get_fk_solver_info");
ros::service::waitForService("ur5_UR5_chain_kinematics/get_fk");
ros::ServiceClient query_client =
rh.serviceClient<kinematics_msgs::GetKinematicSolverInfo>
("ur5_UR5_chain_kinematics/get_fk_solver_info");
ros::ServiceClient fk_client = rh.serviceClient
<kinematics_msgs::GetPositionFK>("ur5_UR5_chain_kinematics/get_fk");
// define the service messages
kinematics_msgs::GetKinematicSolverInfo::Request request;
kinematics_msgs::GetKinematicSolverInfo::Response response;
if(query_client.call(request,response))
{
ROS_INFO("number of joints on service %i",
response.kinematic_solver_info.joint_names.size());
ROS_INFO("number of links on service %i",
response.kinematic_solver_info.link_names.size());
ROS_INFO("Link in question: %s",
response.kinematic_solver_info.link_names[0].c_str());
for(unsigned int i=0;
i< response.kinematic_solver_info.joint_names.size(); i++)
{
ROS_INFO("Joint: %d %s", i,
response.kinematic_solver_info.joint_names[i].c_str());
}
}
else
{
ROS_ERROR("Could not call query service");
ros::shutdown();
exit(1);
}
// define the service messages
kinematics_msgs::GetPositionFK::Request fk_request;
kinematics_msgs::GetPositionFK::Response fk_response;
fk_request.header.frame_id = "base_link";
fk_request.fk_link_names.resize(1);
fk_request.fk_link_names[0] = "wrist_3_link";
ROS_INFO("Forward request call made");
fk_request.robot_state.joint_state.position.resize
(response.kinematic_solver_info.joint_names.size());
// ROS_INFO("joint name resized");
fk_request.robot_state.joint_state.name =
response.kinematic_solver_info.joint_names;
// ROS_INFO("Joint names passed");
for ...