# Sensor-Guided Assembly of Segmented Structures with Industrial Robots
## 3 Problem Statement and Solution Approach
### Resolved Motion with Quadratic Programming  
The traditional teach-and-playback operation for industrial robots is not amenable for sensor-guided motion. Instead, we use the external joint position command, such as the EGM mode in the ABB robot controller, to control the robot motion. Denote the robot joint position vector as \(q\) and commanded joint position as \(q_{c}\). Consider a desired robot end effector spatial velocity \(V^{(d)}\) (stacked angular and linear velocities). Our goal is to determine the joint motion to best match with \(V^{(d)}\) while satisfying a set of inequality constraints, \(h_{I}(q)\), and joint velocity inequality constraints. Inequality constraints on \(q\) ensure the avoidance of singularities (which could cause excessive joint velocities). Configuration space inequality constraints \(h_{I}(q)\) are composed of joint limits and a distance threshold \(d_{min}\) that prevents collision with obstacles. For the robot motion, our approach is a combination of a global path planner and local reactive motion control. The global planner generates a desired task motion spatial velocity, \(V^{(d)}\), that satisfies all the pre-specified constraints. The local planner solves an optimization problem to find the joint motion that best meets the task space motion requirement while satisfying all the constraints. To avoid solving an optimal control problem, the inequality constraints are converted to differential constraints for Jacobian-based feedback velocity control. The end effector velocity \(V^{(d)}\) may also include a user command component (through an input device such as a joystick). Because of the guard against collision and singularities, we call this process safe teleoperation. Based on our previous work in [31; 32], we pose the problem as a quadratic program (QP):  
\[\min_{\hat{q},\alpha_{r},\alpha_{p}}||J\hat{q}-\alpha V^{(d)}||^{2}+\epsilon_{ r}(\alpha_{r}-1)^{2}+\epsilon_{p}(\alpha_{p}-1)^{2} \tag{1}\]  
subject to  
\[\frac{dh_{E_{i}}}{dt} =-k_{E_{i}}h_{E_{i}},\ i=1,\ldots,n_{E}\] \[\frac{dh_{I_{i}}}{dt} =\frac{\partial h_{I_{i}}}{\partial q}\hat{q}\geqslant\sigma_{i} (h_{I_{i}}),\ i=1,\ldots,n_{I}\] \[q_{\min} \leq q\leq q_{\max},\]  
where \(J\) is the Jacobian matrix of the robot end effector, \(\hat{q}\) is the robot joint velocity, and \((\alpha_{r},\alpha_{p})\) specify the velocity scaling factor for the angular and linear portions (constrained to be in \([0,1]\)) with the corresponding weights \((\epsilon_{r},\epsilon_{p})\). The function \(\sigma_{i}\) is a control barrier function [33] that is positive when \(h_{I_{i}}\) is in close proximity to the constraint boundary (i.e., zero). The inequality constraint \(h_{I}\) contains joint limit and collision avoidance. We use the Tesseract trajectory optimization package [34], recently developed by the Southwest Research Institute, to compute the minimum distance, \(d\), between the robot and the obstacles, and the location on the robot, \(p\), corresponding to this distance. The partial robot Jacobian from joint velocity to \(\dot{p}\), \(J_{p}\), is used in the inequality constraint for the QP solution. For our testbed, collision checking is performed at about 10 Hz with geometry simplification.  
To avoid excessive joint acceleration, we use the joint velocity in the previous frame \(\dot{q}^{(-1)}\) to impose the joint acceleration bound \(\dot{q}_{bound}\):  
\[\dot{q}^{(-1)}-\dot{q}_{bound}\ t_{s}\leqslant\dot{q}\leqslant\dot{q}^{(-1)}+ \dot{q}_{bound}\ t_{s}, \tag{2}\]  
where \(t_{s}\) is the sampling rate for updating the joint velocity.