Controlling a real robot with follow_joint_trajectory
Hi all,
I am trying to control my real robot using moveit. More specifically, I am trying to control the robot with follow_joint_trajectory, as I intend to control the robot along different control points. Once I change the position of my robot in rviz using the interactive marker, my "follow_joint_trajectory/feedback" topic gives the below output which stops as the robot in rviz reaches the interactive marker.
My expectation is that the follow_joint_trajectory/feedback gives one controll point to the robot. In this case the first desired position [7.508441107347599e-05, -0.000435379029870564, -0.0014877218024729189, -0.009782363570564764, 0.011565336050273878] and wait until the actual position gets equal to the desired position. And then continues with the next control point. However, the topic just keeps iterating without waiting for the actual position of my robot. What am I missing here?
header:
seq: 307
stamp:
secs: 1613405011
nsecs: 498054023
frame_id: ''
status:
goal_id:
stamp:
secs: 1613405011
nsecs: 397575895
id: "/move_group-4-1613405011.397575895"
status: 1
text: ''
feedback:
header:
seq: 0
stamp:
secs: 1613405011
nsecs: 486645627
frame_id: ''
joint_names:
- my_jnt0
- my_jnt1
- my_jnt2
- my_jnt3
- my_jnt4
desired:
positions: [7.508441107347599e-05, -0.000435379029870564, -0.0014877218024729189, -0.009782363570564764, 0.011565336050273878]
velocities: [0.0, 0.0, 0.0, 0.0, 0.0]
accelerations: [0.0, 0.0, 0.0, 0.0, 0.0]
effort: []
time_from_start:
secs: 0
nsecs: 0
actual:
positions: [0.0, 0.0, 0.0, 0.0, 0.0]
velocities: [0.0, 0.0, 0.0, 0.0, 0.0]
accelerations: []
effort: []
time_from_start:
secs: 0
nsecs: 0
error:
positions: [7.50844110735116e-05, -0.0004353790298701199, -0.0014877218024729189, -0.009782363570565167, 0.011565336050273878]
velocities: [0.0, 0.0, 0.0, 0.0, 0.0]
accelerations: []
effort: []
time_from_start:
secs: 0
nsecs: 0
....
header:
seq: 365
stamp:
secs: 1613405014
nsecs: 398057673
frame_id: ''
status:
goal_id:
stamp:
secs: 1613405011
nsecs: 397575895
id: "/move_group-4-1613405011.397575895"
status: 3
text: ''
feedback:
header:
seq: 0
stamp:
secs: 1613405014
nsecs: 286652256
frame_id: ''
joint_names:
- my_jnt0
- my_jnt1
- my_jnt2
- my_jnt3
- my_jnt4
desired:
positions: [7.667915230921688e-05, 0.028271971524460877, -0.1770854107308687, 0.0031834991701641794, 0.14573102577506053]
velocities: [4.500386941585978e-06, 0.001659314267696707, -0.010393344814048189, 0.00018684319873779262, 0.008553120184965513]
accelerations: [-5.407182794377977e-05, -0.019936542513372818, 0.12487529624453142, -0.0022449076992155544, -0.10276512864934761]
effort: []
time_from_start:
secs: 0
nsecs: 0
actual:
positions: [0.0, 0.0, 0.0, 0.0, 0.0]
velocities: [0.0, 0.0, 0.0, 0.0, 0.0]
accelerations: []
effort: []
time_from_start:
secs: 0
nsecs: 0
error:
positions: [7.667915230946676e-05, 0.028271971524461037, -0.1770854107308688, 0.003183499170163806, 0.1457310257750608]
velocities: [4.500386941585978e-06, 0.001659314267696707, -0.010393344814048189, 0.00018684319873779262, 0.008553120184965513]
accelerations: []
effort: []
time_from_start:
secs: 0
nsecs: 0
Edit:
void des_vels(const control_msgs::FollowJointTrajectoryActionGoal &msg)
{
// for loop the msg.goal.trajectory.points.positions and
// Move the motor and compare with the values from analogRead()
// increase the interator if the discrepancy smaller than threshold.
}
void callback(const my_hardware_interface::Floats_array::Request & req, my_hardware_interface::Floats_array::Response & res)
{
// Simulate function running for a non-deterministic amount of time
int achieve_in_seconds = 30000;
float sensorValue_joint1_real = analogRead(A8);
float sensorValue_joint2_real = analogRead(A10);
float sensorValue_joint3_real = analogRead(A11);
float sensorValue_joint4_real = analogRead(A12);
float sensorValue_joint5_real = analogRead(A13);
res.real_joint1 = (sensorValue_joint1_real);
res.real_joint2 = (sensorValue_joint2_real ...
i'm trying something similar, i think your time_from_start is wrong look for something like
Please do not bump your question needlessly. Your last edit does not seem to change anything.
Re: your question: the
feedbacktopic is not meant to control anything. It is intended to publish feedback (ie: current state). Your assumption about it "stopping" or "waiting" until some external system has caught up is incorrect.The
goaltopic carries the actual trajectory which the controller will execute.But note: this is an actionlib interface. You are not supposed to use individual topics. Instead, create a
FollowJointTrajectoryaction server which communicates with your robot and acceptsFollowJointTrajectoryaction goals (which contain aJointTrajectory).You may also want to look at
ros_control, as it provides a default (and pretty usable) implementation of such a server, reducing your work to writing ahardware_interfacefor your robot.Thank you for the advise. As you can see, I have tried to implement the action server on my arduino side. So using this
callback()function, I can read the response i.e. the joint angle values from the client side. However, I am still not certain of one thing: if we assume that the client has published four goal points throughFollowJointTrajectoryActionGoalas posted, how does the server know how to iterate through these points. Is another step as described indes_vels()necessary or is there any function from ros side for this purpose?You appear to only subscribe to the
goaltopic. That's going to confuse action clients I believe.You'll probably have to implement a bit more of the
actionlibmachinery to make everything work nicely together.your server will have to implement whatever implementation scheme is needed for your robot to move "smoothly" (or whatever criteria you want to use). For some robots, that means interpolation, for others, other additional work must be done.
You could configure whatever produces your trajectories for very dense trajectories. But clients are free to send whatever they want, so you cannot rely on them always sending ...(more)