can not set joint value rightly using setJointPositions or setJointGroupPositions

asked 2018-07-05 20:38:19 -0500

Hanker_SIA gravatar image

updated 2018-07-05 22:39:40 -0500

jayess gravatar image

In a word, the problem is the joint name read from joint_model_group->getJointModelNames() has 8 members which is j2n6s300_joint_base, j2n6s300_joint_1, ... , j2n6s300_joint_6, j2n6s300_joint_end_effector. Then, when i tried to set joint value according joint name, like the follow method(p.s. i set a std::vector forKJointValue(6,0.0) as the getForK() input variable):

for(char i=0;i<joint_value.size();i++)
{
     kinematic_state->setJointPositions(joint_names[i+1], &joint_value[i]);
}

Ideally, i would only set joint value of j2n6s300_joint_1, ... , j2n6s300_joint_6. But, i got follow results

[ INFO] [1529633314.430620000]: Read Joint i:0 j2n6s300_joint_base: -1.478416
[ INFO] [1529633314.430661300]: Read Joint i:1 j2n6s300_joint_1: 2.924741
[ INFO] [1529633314.430705143]: Read Joint i:2 j2n6s300_joint_2: 1.001931
[ INFO] [1529633314.430739710]: Read Joint i:3 j2n6s300_joint_3: -2.080089
[ INFO] [1529633314.430767370]: Read Joint i:4 j2n6s300_joint_4: 1.445890
[ INFO] [1529633314.430795050]: Read Joint i:5 j2n6s300_joint_5: 1.323290
[ INFO] [1529633314.430823610]: Read Joint i:6 j2n6s300_joint_6: 0.000000
[ INFO] [1529633314.430857823]: Read Joint i:7 j2n6s300_joint_end_effector: 0.000000

it means that i did not set joint value rightly. A case in point is j2n6s300_joint_base has been set as the value of j2n6s300_joint_1. Generally, the value of target joint has a shift. BTW, I also test the follow method:

void PickPlace::getForK(std::vector<double> &joint_value, geometry_msgs::Pose &m)
{
    ROS_INFO("Forward Kinematic test");
    robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(robot_model_));
    kinematic_state->setToDefaultValues();
    const robot_state::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("arm");
    const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();  

    std::vector<double> table_joint_value(8,0.0);

    for(char i=0;i<joint_value.size();i++)
    {
        table_joint_value[i+1] = joint_value[i];
    }
    std::cout<<"write joint array:";
    for(int i=0;i<8;i++)
    {
        std::cout<<table_joint_value[i]<<" ";
    }
    std::cout<<std::endl;

    kinematic_state->setJointGroupPositions(joint_model_group,table_joint_value);   

    std::vector<double> joint_values;
    kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
    for(std::size_t i = 0; i < joint_names.size(); i++)
    {
        ROS_INFO("Read Joint i:%d %s: %f", i, joint_names[i].c_str(), joint_values[i]);
    }  
    const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("j2n6s300_end_effector");
    /* Print end-effector pose. Remember that this is in the model frame */
    //ROS_INFO_STREAM("Translation: " << end_effector_state.translation());
    //ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation());   
    tf::poseEigenToMsg(end_effector_state,m);
    ROS_INFO_STREAM("Pose Computed:"<<m);
}

Unfortunately, i did not get right result either. Thanks for your advice!

edit retag flag offensive close merge delete