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 <&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&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;isetJointPositions(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 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 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:"<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;isetJointPositions(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
Although i tried different methods, but i can not solve this problem. But when i ignore this problem, the position of distal joint seems right, because i displayed point computed by getForK() using rviz marker. While i am confused about all of this. Looking forward your advice! Thanks a lot!
↧