Open pangtao22 opened 3 years ago
@weigao95 mentioned that the pointer to Plan
retrieved here can be invalidated when the Queue that holds the Plan reaches its capacity and re-allocates memory, which can happen if a new plan is queued while the current plan is executed. Although I don't think this can happen under the current finite state machine logic, it is important to keep this failure mode in mind when we change the logic, which will probably happen as we resolve #21 .
The new lcm type
lcmt_our_robot_plan
(#15) allows sending multiple plans to the robot in one message, which is indeed a valid use case. For example, in the labs of 6.881, students need to run multipleJointSpaceTrajectoryPlan
s generated by a sequence of IK solves. In one of my own projects, I usually prepend a more interestingPlan
with aJointSpaceTrajectoryPlan
to make sure that the more interestingPlan
always starts at the same initial configuration.The previous solution to executing consecutive plans is to queue them up in the client, send one to the server (
IiwaPlanManager
), wait for the server to reply a success token, and then send the next plan. This introduces an indeterministic time interval between the end of one plan and the beginning of the next. The indeterminacy is partly due to network communication, and partly due to the inaccuratestd::this_thread::sleep_for()
function currently used to enforce the update frequency of the plan-handling thread.Among its many other side effects, the indeterminacy makes it more cumbersome to compute the mean and variance of executing the same plan for 50 times, as each Plan starts at a slightly different time.
On the other hand, queuing the
Plan
s up inIiwaPlanManager
eliminates this indeterminacy, as plan switching is timed by IIWA_STATUS messages, which we believe is a more accurate clock signal.The proposed state machine behavior for executing multiple plans, which is consistent with the current plan-switching logic, is as follows:
Plan
s. In other words, the state machine is in state RUNNING from receiving the list ofPlan
s to the end of the lastPlan
in the list or when an error occurs, whichever happens earlier.Plan
s are successfully executed, a successful token is sent to the client after allPlan
s are completed.Plan
where error occurred.lcmt_our_plan_status
.We can discuss this in the weekly meeting on Thursday.