Problem of setting value of arm joint by moveit setJointPositions

asked 2018-06-28 02:14:26 -0600

Hanker_SIA gravatar image

updated 2018-06-28 02:48:01 -0600

When we request a pose to moveit api, we can get the plan result, As following: group_->setPoseTarget(start_pose_); my_plan= evaluate_plan(*group_); I modified the evaluate_plan function so that i can let plan result return. Then i print part information of the plan result, as following:

int joint_name_size, plan_point_size;
joint_name_size = my_plan.trajectory_.joint_trajectory.joint_names.size();   
plan_point_size = my_plan.trajectory_.joint_trajectory.points.size();
std::cout <<"joint_name size:"<< joint_name_size <<std::endl;
std::cout <<"points  size:"<< plan_point_size <<std::endl;
for(int i=0; i< joint_name_size;i++)
{
    ROS_INFO("joint name:%s", my_plan.trajectory_.joint_trajectory.joint_names[i].c_str());
}
ROS_INFO_STREAM("Last point of Plan:"<<my_plan.trajectory_.joint_trajectory.points[plan_point_size-1]);

So, we can know the joint name and a points demo, as following:

joint_name size:6
points size:30
[ INFO] [1529570085.394579180]: joint name:j2n6s300_joint_1
[ INFO] [1529570085.394606035]: joint name:j2n6s300_joint_2
[ INFO] [1529570085.394628844]: joint name:j2n6s300_joint_3
[ INFO] [1529570085.394651328]: joint name:j2n6s300_joint_4
[ INFO] [1529570085.394673488]: joint name:j2n6s300_joint_5
[ INFO] [1529570085.394695385]: joint name:j2n6s300_joint_6
[ INFO] [1529570085.394770602]: Last point of Plan:positions[]
positions[0]: -2.19888
positions[1]: 3.73149
positions[2]: 1.84508
positions[3]: -2.77105
positions[4]: 0.407463
positions[5]: 2.5911
velocities[]
velocities[0]: 0
velocities[1]: 0
velocities[2]: 0
velocities[3]: 0
velocities[4]: 0
velocities[5]: 0
accelerations[]
accelerations[0]: 0.559416
accelerations[1]: -0.627759
accelerations[2]: -0.652831
accelerations[3]: 0.54093
accelerations[4]: 0.800661
accelerations[5]: -0.988729
effort[]
time_from_start: 4.042044055

Then, i wrote a function used to solve forward kinematic model as follows:

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();  
    // for(std::size_t i = 0; i < joint_names.size(); i++)
    // {
    //     ROS_INFO("Write Size: %dJoint i:%d %s: %f", joint_names.size(), i, joint_names[i].c_str(), joint_value[i]);
    // }
    // kinematic_state->setToRandomPositions(joint_model_group);
    for(char i=0;i<joint_value.size();i++)
    {
        kinematic_state->setJointPositions(joint_names[i+1], &joint_value[i]);
    }
    //ROS_INFO("Joint i:%d %s: %f", i, joint_names[i].c_str(), joint_value[i]);
    // std::vector<double> joint_values_(7, 0.0);
    // 
    //j2n6s300_joint_1 joint_values_[0] ;
    //j2n6s300_joint_2 joint_values_[1] ;
    //j2n6s300_joint_3 joint_values_[2] ;
    //j2n6s300_joint_4 joint_values_[3] ;
    //j2n6s300_joint_5 joint_values_[4] ;
    //j2n6s300_joint_6 joint_values_[5] ;
    // kinematic_state->setJointGroupPositions(joint_model_group,joint_values_);   
    // kinematic_state->setJointPositions(joint_names[0],angle);
    // kinematic_state->setToRandomPositions(joint_model_group);
    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);
}

While, The first 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 ...
(more)
edit retag flag offensive close merge delete