Cleaned the code (a bit) and use macro names instead of hardcoded string for the sems
Added a def to select between closed or open loop
Added assert inside command and status
Merging with the ik
Dirty but working ik