can not set joint value rightly using setJointPositions or setJointGroupPositions
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!