Preliminary planning and control algorithms for safe manipulation

Preliminary software deliverable to provide a controller that guarantees safety and stability of task execution according to the safe velocity from risk- and human-aware SMU and kinematic constraints based on risk factors.