Currently, my sendgoal have a move_group interface solemly for my robot's arm and now i am adding in a new sets of codes to move my finger simultaneously.
I have added a duplicate of the right arm's FollowJointTrajectoryActionGoal codes and adjusted it to fit my finger.
Everything complied nicely, but when i run the sendgoal node. My simulation(gazebo) died/crash.
Below is a show of my current codes, Goalf is for the finger,
//finger
// Move the robot (in simulator)
for (int num_idx = 0; num_idx < num_positions_to_move; num_idx++) // each step in trajectory
{
goalf.goal.trajectory.points[num_idx].positions.resize(gf_joint_names.size());
for (int i = 0; i < gf_joint_names.size(); i++)
goalf.goal.trajectory.points[num_idx].positions[i] = target_locf[i];
// print the movement
for (int k = 0; k < gf_joint_names.size(); k++) // relay the movement in joint angles [rad] to the user
printf("%f, ", goalf.goal.trajectory.points[num_idx].positions[k]);
printf("-- \n");
// move every dt=T_STEP seconds --> i.e. speed = eef_step/dt
goalf.goal.trajectory.points[num_idx].time_from_start = ros::Duration(T_STEP * (num_idx+1));
}
// finally publish goal, count and sleep according to movement frequancy
// goal_pub.publish(goal);
goal_pubf.publish(goalf);
Please advice on whats wrong.
Thank you in advance.
↧