kaust wrote:How can an already set stop condition be cleared when the robot stops due to it?
the simulator holds a set of stop conditions. this set is initially empty. the user can specify a new set of stop conditions by calling the set_stop_conditions service.
when the robot is executing a move_along_joint_path at every simulation timestep, each condition is checked - if one condition is met (i.e. measured est. torque of 2nd lwr joint is above 30Nm) then the movement is stopped and the cause of the stop is returned in the stop_reason field of the move_along_joint_path service call.
if you want to clear the set of stop conditions simlpy call the set_stop_conditions service with an empty conditions-field.
example:
1. set the set of stop conditions to hold only a single stop condition which triggers as soon as the 2nd joint of the LWR measures an external torque higher than +30 Nm:
- Code: Select all
system.set_stop_conditions([
StopCondition(
condition_type='joint_ext_torque_threshold',
joint_name='lwr_joint_2',
condition_operator='>',
threshold=30.0
)
])
2. now move the lwr into a direction which will cause the stop condition to trigger because of a collision e.g. with the table
- Code: Select all
# assume the lwr is at it's 0,0,0,0,0,0,0 position
stop_reason = system.move([Configuration(q=[110])], ['lwr_joint_2'], lwr_acc=100)
print 'Move finished, stop reason: ' + stop_reason
this will stop the lwr at ~106[deg] and will print that the movement was stopped because "stop_condition 0: joint 1 ext_torque 30.1747 > 30.0000 at sim_time ..." (sadly in this output the joint's are zero-indexed.)
3. to be able to move the lwr out of this collision you first need to remove this stop-condition. otherwise it will immediately trigger again on the next move_along_joint_path call. here we simply set an empty set of stop conditions, and then command the robot back to it's 0-position:
- Code: Select all
system.set_stop_conditions([])
stop_reason = system.move([Configuration(q=[0])], ['lwr_joint_2'], lwr_acc=100)
print 'Move finished, stop reason: ' + stop_reason
which should move the lwr and then print the stop reason as "path finished".