Towards Tethered Tool Manipulation Planning with the Help of a Tool Balancer ^†

Abstract

Handling and maneuvering tools across a robot workspace is a challenging task that often requires the implementation of constrained motion planning. In the case of wired or tethered tools, their maneuvering becomes considerably harder by the tool cable. If the cable presence is not considered, the robot motions may make the cable become entangled with the robot arms or elements of its workspace, causing accidents or unnecessary strain on the robot and the tool. Furthermore, the behavior of the tool cable during manipulation and its degree of entanglement around the robot are difficult to predict. The present paper introduces a constrained manipulation planner for dual-armed tethered tool manipulation involving tool re-grasping. Our solution employs a tool balancer to straighten the tool cable and facilitate the cable deformation problem. The planner predicts the cable states during manipulation and restricts the robot motions in order to avoid cable entanglements and collisions while performing tool re-posing tasks. Furthermore, the planner also applies orientational constraints to limit the cable bending, reducing the torque and stress suffered by the robot due to the cable tension. Simulations and real-world experiments validated the presented method.

1. Introduction

Recently, the inclusion of robots in the manufactory industry has reduced human workloads, increased productivity, and reduced costs in many industries. However, industrial robots are often found in enclosed and controlled environments with minimal or null human presence. In industrial scenarios, a robot usually has to handle tools to execute different tasks. In those cases, the tool’s inherent characteristics such as its weight, shape, and cabling represent hindrances for tool manipulation. A tool’s cable, in particular, represents a difficult problem for robotic manipulation: the robot might become entangled with the cable, causing damage to the tool or itself, or it might excessively bend the cable causing wear and tear or its breakage, as shown in Figure 1.

This paper presents a manipulation planning solution for the manipulation of tethered tools. The proposed planner uses an entanglement quantification method to estimate the degree of entanglement between the robot and a tool cable. The planner keeps track of the changes in cable orientation, and its relation to the grasping angle of the tool to compute the maximum rotation the cable could have around the robot arms. By keeping this rotation under a given threshold, the planner minimizes robot–cable entanglements and reduces the strain caused by the cable on the robot joints. Furthermore, the planner also limits the bending of the cable during manipulation, reducing the torque caused by the cable on the robot joints. Accordingly, we review the related publications, including those addressing motion planning and manipulation planning.

1.1. Motion Planning

Motion planning is a widely researched topic in robotics. Seminal work on motion planning include path planning algorithms [1], techniques for establishing lower bounds in robot motion planning problems [2], motion planning with fuzzy logic [3,4], the use of neural networks for planning [5,6], and the inclusion of genetic algorithms in motion planning algorithms [7,8,9].

More recently, newer solutions for motion planning have been presented. In [10], a data-driven approach for motion planning is proposed. A demonstration-guided motion planning framework was developed by Ye and Alterovitz [11]. In [12], a robot-specific circuitry for motion planning building process is presented.

In the field of motion planning, maneuvering and manipulating objects represents a difficult challenge. Manipulation planning is often considered to be a constrained case of motion planning—the manipulated object represents a dynamic obstacle that the robot must avoid. The motion sequences generated by manipulation planning allow a robot to move objects and to modify its environment’s structure.

1.2. Manipulation Planning

Manipulation consists in finding a series of intermediate object poses to pick-up an object in a given starting pose and re-pose it in a goal pose. Early work in manipulation solutions for robotic arms [13,14,15] explore regrasp planning in fixed single-armed robots. These work present some guidelines for future research in manipulation planning.

More recently, multiple works on manipulation planning have been presented. These work include single- and dual-arm algorithms for regrasping [16,17,18], a motion planning architecture on a hardware-in-the-loop application [19], a planner that computes robot configurations to grasp assembly parts for sequences of collaborative assembly operations [20], and 3D object reorientation using pivoting [21].

On the other hand, robot manipulation planning involving cable-like objects has presented many challenges; these objects are difficult to manipulate, with often unpredictable deformations and, in the case of cables, they can become entangled around the robot or its surroundings. Several works centered on manipulation planning have tackled different problems associated with deformable objects such as cables, chains, hoses, etc. For example, Hert et al. [22] developed a planner for multiple tether robots in 2D space, where the tails are treated as lines or bounded segments. Igarashi et al. [23] also presented a 2D planner for a mobile robot with a tail. The tails are treated as a manifold in the configuration space. Bretl et al. [24] approximated flexible wires locally using a chart in the 3D world and demonstrated its usage in quasi-static manipulation planning. Ramirez et al. [25] developed a finite element model for a fire hose and used it to generate the pulling motion of a humanoid robot. In [26], a framework for cable shapes manipulation using a deformation model is shown. A motion planner for manipulating deformable linear objects and tying knots was described by Saha and Isto [27], while Khalil and Payeur [28] reviewed the works on dexterous robotic manipulation of deformable objects by using sensor feedback.

1.3. Contribution

The aforementioned worka on object manipulation suggest general solutions for object reorientation, but the wires of tools are overlooked. It remains difficult to include cable shape prediction, collision, and entanglement avoidance as well as motion planning in the same loop. This paper uses a tool balancer to convert the cable constraints into a straight line, bypassing the necessity of modeling the cable to predict its behavior. It also implements a constrained planner that addresses the problems caused by wired cables using movement constraints. The constraints limit excessive bending of the cable, reducing stress on the robot joints. They also help the robot avoid becoming entangled with the tool cable as well as avoid accidents related to cable snarling. This paper is an extension and continuation of our previous work [29].

2. Materials and Methods

2.1. Real-world Setting

Our robot environment simulates an industrial workspace. Figure 2 shows the robot handling a suspended tool. The tool is attached to a tool balancer, a device designed to suspend tools and aid factory workers by reducing the load of carrying a tool. The tool balancer provides a constant pulling force that straightens the cable. The robot counts with hand-mounted cameras used to detect AR markers placed on a tool. The object initial pose is calculated from the AR marker position and orientation. We use our simulation environment along with our manipulation planner to plan a robot manipulation sequence to place the object in a desired pose.

2.2. Simulation Setting

The simulation environment (https://gitlab.com/wanweiwei07/wrs_nedo) is used to simulate robot–object–environment interactions and plan robot manipulation motion sequences (Figure 3). The simulation environment uses 3D meshes to represent the tool and the tool balancer in the robot environment. The tool original pose is determined with the robot hand-mounted cameras and the tool AR marker [30].

2.3. Manipulation Planner

Our planner [16] uses a pre-built database of grasps, object placements, and handover pairs of grasps to build a graph of grasps. When we give a start and goal object pose to the planner, it searches the grasp graph to find IK-feasible poses associated to the initial and final grasps of the object.

For handover execution, the planner also searches the graph to find pairs of grasps that are compatible with the starting and final grasp poses. If a compatible set of initial grasp–handover–final grasp is found, the planner uses RRT to connect these key poses and create a motion sequence for the robot to complete its task. It is also possible for the planner to find solutions that involve more than one handover operations where the object moves from hand to hand several times, which involves more key poses. Naturally, there is often more than one solution or set of compatible pick up–handover–place down grasps to complete the task. In that case, the planner chooses randomly between the options that involve the fewest key poses. In the event that a complete motion sequence cannot be found for a given set of key poses (due to unavoidable collisions in the motion sequence or an inability for the robot to preserve its balance), the planner selects another set of key poses and starts the RRT exploration again.

The generated motion sequence allows the robot to re-pose a tool in a desired goal pose. However, in the case of tethered tools, it is necessary to impose constraints on the planning stage to limit the bending of the cable and avoid entanglements with the robot or its environment. The following sections discuss these constraints. They are added to our planner developed in [16] to ensure neither arm becomes entangled. The dual-arm planner is sequential: the constraints are added to each single arm, respectively.

3. Constraints for Entanglement Avoidance

To prevent entanglements around the robot, we have implemented two constraint-based solutions to our planner. The first set of constraints, presented in [29], uses orientation constraints to prevent the robot from bending the tool cable past a certain threshold, reducing the torque suffered by the robot during manipulation. Our second set of constraints makes use of a proposed method to measure the possible snarling or “angle accumulation” of the cable around the robot arm. In this case, the planner imposes a maximum threshold on angle accumulation, avoiding entanglements during manipulations. Both methods were implemented in our planner.

3.1. Orientation Constraints

Our planner uses orientational constraints to limit the bending of the tool cable around the tool itself during the manipulation of the tool. For every robot–tool state calculated during sampling-based motion generation, the planner measures the cable bending. If the bending surpasses a given threshold, the planner discards said poses and searches a new pose that complies with the threshold. The process is repeated until a complete motion path is found. The planner uses two vectors ${\alpha }^{\circ }$ and ${\beta }^{\circ }$ in the robot reference frame ${\mathrm{\Sigma }}_{\mathrm{o}}$ to measure the bending of the cable. ${\alpha }^{\circ }$ is a reference vector and ${\beta }^{\circ }$ represents the vector formed by the cable straight line shape.

3.1.1. Computation of the Bending Angles

To calculate the bending of the cable for a given robot–tool pose, the planner first computes the vectors ${\alpha }^{\circ }$ and ${\beta }^{\circ }$. For any given tool pose, the planner calculates the vector ${\alpha }^o$ by inserting the vector ${}_{}^{\mathrm{t}}{\upsilon }_{}^{}={}_{}^{\mathrm{t}}z$ into Equation (1).

$${}_{}^{\circ }{\upsilon }_{}^{}={}_{}^{\circ }{\mathbf{R}}_{\mathrm{t}}^{}{}_{}^{\mathrm{t}}{\upsilon }_{}^{}$$

(1)

where ${}_{}^{\circ }{\mathbf{R}}_{\mathrm{t}}^{}$ is the object’s rotational matrix, ${}_{}^{\mathrm{t}}{\upsilon }_{}^{}$ is a given vector in the tool’s reference frame ${\mathrm{\Sigma }}_{\mathrm{t}}$, and ${}_{}^{\circ }{\upsilon }_{}^{}$ represents the same vector in the robot’s frame of reference ${\mathrm{\Sigma }}_{\mathrm{o}}$. In our case, we chose ${}_{}^{\mathrm{t}}{\upsilon }_{}^{}={}_{}^{\mathrm{t}}z$ since the z-axis tool reference frame aligns with the tool connection point normal vector; the resulting vector ${}_{}^{\circ }{\upsilon }_{}^{}$ is equal to the reference vector ${}_{}^{\circ }\alpha$.

The vector ${\beta }^{\circ }$ is calculated by computing the straight line formed by the cable. The line starts at the tool connection point and ends at the tool balancer connection point, as shown in Figure 4.

3.1.2. Angular Measurement of Cable Bending

Our planner uses vectors ${\alpha }^{\circ }$ and ${\beta }^{\circ }$ to measure the bending angle $\theta$. The angle measurement is performed for every robot–tool state generated by the planner during the sampling-based motion generation (in our case, RRT). To compute the angular difference $\theta$, the planner uses Equation (2)

$$\theta =arccos\frac{{}_{}^o{\alpha }_{}^{}·{}_{}^o{\beta }_{}^{}}{\left|{}_{}^o{\alpha }_{}^{}\right|\left|{}_{}^o{\beta }_{}^{}\right|}$$

(2)

to limit the bending of the cable, we set a maximum threshold, ${\theta }_{\mathit{t}}$, for the value of $\theta$. If a given robot–tool state generated during RRT exploration has a value $\theta$ that surpasses ${\theta }_{\mathit{t}}$, the planner considers the cable to be excessively bent, violating our constraint. In such cases, the planner discards the robot pose and continues searching for robot poses that do not violate the constraint until a complete motion path without excess cable bending is found. Figure 4 presents our simulation environment with different tool orientations and illustrates vectors ${}_{}^{\circ }{\alpha }_{}^{}$ and ${}_{}^{\circ }{\beta }_{}^{}$ and their angular difference.

3.2. Angle Accumulation Constraints

The orientation constraints offer a direct solution to limit the bending of the tool cable during manipulation. However, the angle constraints do not directly prevent cable entanglements. To address cable entanglements, we use a method to predict the magnitude of cable “snarling” or cable movement around the robot arm during simulation [31]. By measuring the angle accumulation of every robot state during the planning stage and preventing the accumulation from surpassing a given threshold, it is possible to create entanglement-free motion sequences for tool manipulation.

The concept of angle accumulation [31] represents the degree of rotation or movement of the tool cable around the robot arm. For simplicity, our planner uses polar coordinates in the tool reference frame to quantify the movement of the cable. The angle accumulation is measured by computing the azimuthal and polar rotations of the cable within an “accumulation region”. The accumulation region is determined by the polar angle used by the robot hand to grasp the tool ${\varphi }_H$ and the azimuthal positioning of the robot hand, as shown in Figure 5.

When the cable enters the accumulation region (red region in the figure) the planner considers the cable to be in a state of angle accumulation and starts recording the changes in polar and azimuthal angles of the cable to determine if the movements of the cable contribute to get itself entangled around the robot arm or the tool. Our planner can make use of the angle accumulation estimation to inspect individual robot–cable estates/poses during the planning stage. Similar to the orientation constraints, we can introduce a threshold or maximum permissible angle accumulation to limit the snarling and movement of the cable around the robot arms.

In principle, it is not always possible to calculate the angle accumulation for a single robot state since it depends on the preceding accumulation values and the direction of rotation of the cable. Our planner calculates each state angle accumulation during RRT-based motion planning to create entanglement-free motion paths from an initial robot key pose/state and a goal.

The ith robot–cable state in a motion sequence has an angle accumulation $\mathrm{\Gamma }\left(i\right)$ that depends on the previous states directions of rotation (clockwise or counter clockwise) of the cable. The accumulation $\mathrm{\Gamma }\left(i\right)$ is calculated by comparing the changes of orientation of the $\beta$ vector with Equation (3). It is important to notice that ${\beta }_0$ represents the first cable state that crosses the accumulation region.

$$\mathrm{\Gamma }\left(i\right)=\sum _{j=1}^i((\mathrm{\Theta }\left({\beta }_j\right)-\mathrm{\Theta }\left({\beta }_{j-1}\right))\mu \left({\beta }_{0,1,\dots ,j}\right)+(\varphi \left({\beta }_j\right)-\varphi \left({\beta }_{j-1}\right))\mathrm{\Phi }\left({\beta }_{0,1,\dots ,j}\right)$$

(3)

where the functions $\mathrm{\Theta }\left(\beta \right)$ and $\varphi \left({\beta }_j\right)$ return the azimuthal and polar angles of the ith cable vector ${\beta }_i$, respectively. For simplicity, the cable vector $\beta$ uses the tool reference frame unlike the vector ${}_{}^{\circ }{\beta }_{}^{}$ discussed in the previous subsection; otherwise, both vectors represent the orientation of the cable. The functions $\mu \left({\beta }_{0,1,\dots ,j}\right),\mathrm{\Phi }\left({\beta }_{0,1,\dots ,j}\right)\in \{-1,1\}$ are used to change the sign of the subtraction. Both functions can be seen as memory functions that verify if the change in cable state from ${\beta }_i$ to ${\beta }_{i+1}$ contributes to increasing the accumulation or decreasing it. The functions check if the net contribution from previous cable states results in a clockwise or counterclockwise rotation accumulation and, if necessary, it changes the sign of the operations to increase or decrease the polar or azimuthal accumulation accordingly.

Angle Accumulation Constraints in Sampling-based Motion Planning

To keep track of how the angle accumulates during sampling-based motion planning, we use unidirectional RRT to create nodes/states (robot/object intermediate poses) that connect the starting tool pose to the goal tool pose. The planner stores the orientation of the cable for every robot state ${\beta }_i$ as a vector in the tool reference frame and its accumulation angle. To verify the angle accumulation of a given node, the planner compares the $\beta$ vector of the node with its parent node. The planner reads the direction of rotation (clockwise or counterclockwise) accumulation in the polar and azimuthal planes of the parent node and uses Equation (3) to compute the angle accumulation.

If the angle accumulation for a particular state/node surpasses a given threshold, the state’s node is eliminated from the permissible nodes and a new node is created. The process continues until a set of connected nodes that link the start and goal pose is found or the planning stage times out. In the latter case, a new set of starting and goal poses is searched and the RRT exploration is restarted. Figure 6 better illustrates this process. The figure shows two hypothetical motion paths generated by RRT: The first path allows the robot to take the object from a starting pose and place it in a desired goal pose without surpassing angle accumulation thresholds. The second path makes the robot reach the same goal pose at the cost of high angle accumulation, which may cause entanglements.

The threshold ${\mathrm{\Gamma }}_t$ is used to limit the movement of the cable in the entanglement zone and prevent entanglements. Essentially, by limiting the movement of the cable in the accumulation region shown in Figure 5, we decrease the chance of the cable snarling around the robot arm or colliding with the robot end-effector during handover, as shown in Figure 1.

4. Experiments

Several experiments were performed to test and compare our proposed planner to our original planner [16]. The tests involved different object reposing tasks with distinct starting and goal poses. Simulations and real-world experiments are presented. For the simulations, we provide a running time comparison between the presented solution and our original planner and a comparison between the planners’ rate of success in computing manipulation motion sequences to solve the given tasks. For the real-world experiments, the robot force/torque sensor was used to measure the torque suffered by the robot during manipulation. In addition, we measured the angle accumulation of the cable to compare the possible degree of snarling caused by the tool cable between both planner solutions.

4.1. Simulations

To assess the performance of our constrained planner and compare it to the original, we performed a series of tests in our simulation environment. Firstly, we tested the planners’ success rates by tasking the robot to perform several manipulation tasks with a tethered tool. We designed several benchmarks to test our planner. Each benchmark consists of generating a motion sequence to grasp the tool with the robot left arm, perform a handover, and place the tool in one of the five goal poses shown in Figure 7. We tested our proposed planner using ${\theta }_t=120$° as threshold for the angle bending and ${\mathrm{\Gamma }}_t=90$ for the angle accumulation threshold and compared it to our original motion planner. The results of the tests can be seen in

Table 1. Simulation results for the constrained and unconstrained planner.

Benchmark		Proposed Planner		Unconstrained Planner
1		$\circ \left\{82.65\right\}$		$\times \left\{57.68\right\}$
2		$\circ \left\{87.57\right\}$		$\times /🟉\left\{63.66\right\}$
3		$\circ \left\{80.19\right\}$		$🟉\left\{59.38\right\}$
4		$\circ \left\{85.40\right\}$		$\circ \left\{64.24\right\}$
5		$\circ \left\{80.80\right\}$		$\times /🟉\left\{69.24\right\}$
5,2		⨂		$\times /🟉\left\{67.63\right\}$
1,3		$\circ \left\{196.66\right\}$		$\times \left\{76.91\right\}$
4,3		$\circ \left\{180.11\right\}$		$\times /🟉\left\{84.52\right\}$

Symbols: ∘ is used to represent successful attempts to pose the object in the goal pose without excess bending or angle accumulation. × indicates that the cable was bent above our threshold angle of 120°. 🟉 indicates that the angle accumulation surpassed the threshold $\mathrm{\Gamma }=90$°. ⨂ indicates that the planner could not find a solution within the maximum planning time. The planning time for each completed benchmark is shown inside { } with the time in seconds. The benchmark number indicates which tool goal pose or poses the planner was tasked to place the tool in.

. To calculate the planner’s success rate with different manipulation tasks, we labeled the execution of the tasks using the following criterion: If the tool’s bending angle calculated with Equation (2) was higher than the given threshold angle at any point of the task, the execution was labeled as failed by excessive bending of the cable; if the angle accumulation surpassed ${\mathrm{\Gamma }}_r$, the task was identified as a failed case due to entanglement with the cable.

4.2. Real-world Experiments

After performing the simulations, we tested the planned motion sequences in our real-world robot. The motion sequences computed by our proposed planner can limit the cable bending and the movement of the cable around the robot arm. However, it is necessary to test the sequences in a real-world environment to assess how well the implemented constraints or thresholds in cable bending and angle accumulation translate into motion sequences that produce less torque on the robot arms and avoid cable snarling around the robot.

Therefore, we tasked our real-world robot to complete several of the benchmarks presented in the previous section by using our proposed planner and our original unconstrained planner. Figure 8 shows some of the robot executions. To assess the planners ability to prevent entanglements, we registered the number of entanglement accidents caused by cable snarling. Every accident consists of the cable getting attached around the robot arm and the robot undesirably pulling the cable when performing a handover motion. The upper row of Figure 8 shows an example of an entanglement-related accident. Furthermore, we registered the torque suffered by the robot during the manipulation tasks.

Table 2. Real-world results for the constrained and unconstrained planner.

Benchmark		Proposed Planner		Unconstrained Planner
1,3		∘$[0.25,0.87]$		⊘$[0.84,1.25]$
2		∘$[0.63,0.75]$		⊘$[1.27,1.57]$
3		∘$[0.69,1.01]$		∘$[1.66,1.03]$
4,3		∘$[0.95,0.68]$		⊘$[2.28,2.43]$
5		∘$[0.83,1.09]$		⊘$[1.41,1.56]$

Symbols: ∘ is used to represent successful attempts to pose the object in the goal pose without entanglement-related accidents. ⊘ indicates that the robot suffered an accident during manipulation caused by the cable. Inside $\left[\right]$ we show the average torque $[N·m]$ caused by the cable on the robot end-effector right and left arms, respectively.

shows the results gathered from the experiments, including the registered cable-related accidents during manipulation as well as the mean and maximum torque values registered by the force/torque sensor of the robot UR arms. Figure 9 shows the torque measurements for each benchmark. With the constraints, the total torque resisted by the robot is reduced (see blue vs. green; red vs. yellow). The peaks appear when the robot switches between planned motion segments. The blue and red curves had more peaks than the green and yellow ones, which implies that the constraints increased the complexity of the planned motion. This is consistent with our expectation that the robots are leveraging the motion changes to avoid entanglement. Note that the constraints do not necessarily increase the planning time—the blue and red curves are not always longer than the green and yellow ones. The planning time depends on the obstacle manifolds in the configuration space. In some cases, the constraints guide the robot to circumvent configuration obstacles and the planning time decreases.

5. Discussion

A constrained planner for tethered tool manipulation is presented. The planner limits the bending angle of the tool cable and constrains the cable movement in the vicinity of the robot arms, successfully reducing stress caused by the cable tension as well as avoiding entanglements with the cable. Limiting the bending of the cable can reduce the torque suffered by the robot arms during manipulation and decrease entanglements. With the unconstrained planner, the average torques suffered by the robot arms were $1.56N·m$ and $1.492N·m$ for the right and left arms, respectively; our planner reduced the average torques to $0.88N·m$ and $0.67N·m$, respectively, thus highly reducing the torque suffered by the robot. However, the imposed threshold constraints can reduce the search space for key poses and limit the available robot configurations during planning, considerably increasing the average planning time when compared to the unconstrained planner. Results confirm that the planner can successfully compute motion sequences for robots to handle balancer-suspended tools, preventing collisions with a tool’s cable and its excess bending, thus reducing the strain suffered by the robot by diminishing the torque applied by the balancer’s retractable cable on its joints. The planner is expected to play promising roles in manufacturing cells.