Model-Based Manipulation of Linear Flexible Objects: Task Automation in Simulation and Real World ^†

Abstract

Manipulation of deformable objects is a desired skill in making robots ubiquitous in manufacturing, service, healthcare, and security. Common deformable objects (e.g., wires, clothes, bed sheets, etc.) are significantly more difficult to model than rigid objects. In this research, we contribute to the model-based manipulation of linear flexible objects such as cables. We propose a 3D geometric model of the linear flexible object that is subject to gravity and a physical model with multiple links connected by revolute joints and identified model parameters. These models enable task automation in manipulating linear flexible objects both in simulation and real world. To bridge the gap between simulation and real world and build a close-to-reality simulation of flexible objects, we propose a new strategy called Simulation-to-Real-to-Simulation (Sim2Real2Sim). We demonstrate the feasibility of our approach by completing the Plug Task used in the 2015 DARPA Robotics Challenge Finals both in simulation and real world, which involves unplugging a power cable from one socket and plugging it into another. Numerical experiments are implemented to validate our approach.

1. Introduction

Manipulation of linear flexible objects is of great interest in many applications including service, manufacturing, health, and disaster response. Indeed, the topic has been the focus of many research studies in recent years. Cables, ropes, clothes, organs, and strings are common deformable objects used in deformable object manipulation [1,2,3,4,5,6]. Cables are linear flexible objects that are common both in industrial and domestic environments. Popular tasks about linear flexible object manipulation include tying a knot using a rope [7], inserting a string to a hole [8], untangling a rope tie [9], predicting and controlling the shape of the cable [10,11].

Cables are linear flexible objects that are common in industrial, domestic, and nuclear environments. The 2015 DARPA Robotics Challenge (DRC) Finals was aimed at advancing the capabilities of human-robot teams in responding to natural and man-made disasters. The Plug Task in this challenge required the robot to pull a power cable out of a socket and plug it into another [12]. Among six teams that completed the task, Team WPI-CMU had the fastest completion time with 5 min and 7 s [13]. Their approach used teleoperation by the human operator to grasp the plug and to complete the task [14]. The other five teams also did not automate this task, and they took longer times to perform the task.

In most of the related literature of manipulating cable-like objects, the physical model of the deformed object needs to be known before the manipulation such that the deformation of the object can be predicted. A recent review paper about different physical models of the cable-like deformable linear objects is provided by Lv et al. [15]. Chen and Zheng [16] used a cubic spline function to estimate the contour of a flexible aluminum beam in the 2D plane, and a non-linear model was applied to describe the deformation based on the material characteristics identified by vision sensors. A systematic method to model the bend, twist, and extensional deformations of flexible linear objects was presented in [17], and the stable deformed shape of the flexible object was characterized by minimizing the potential energy under the geometric constraints. Nakagaki et al. [18] extended this work and proposed a method to estimate the force on a wire based on its shape observed by stereo vision. This method was employed for an insertion task of flexible wire into a hole. Linn et al. [19] proposed a discrete model of flexible rods based on Kirchhoff’s geometrically exact theory for VR applications. Caldwell et al. [20] presented a technique to model a flexible loop by a chain of rigid bodies connected by torsional springs. Yoshida et al. [21] proposed a method for planning motions of a ring-shaped object based on precise simulation using the Finite Element Method (FEM). Lv et al. [22] proposed a new mass-spring model by adding torsion springs to the cable links to express twisting behavior.

There are also approaches not based on the physical model of the flexible object. Navarro-Alarcon et al. [23,24,25] proposed a framework for automatic manipulation of deformable objects with an adaptive deformation model in 3D space. Cartesian features composed of points, lines, and angles have been used to represent the deformation. Navarro-Alarcon and Liu [26] proposed a representation of the object’s shape based on a truncated Fourier series, and this model allows the robotic arm to deform the soft objects to desired contours in the 2D plane. Recently, Zhu et al. [11] extended the work in [26] and used the Fourier-based method to control the shape of a flexible cable in 2D space. Compared with the Fourier-based visual servoing method, the SPR-RWLS method with the Gaussian model proposed by Jin et al. [27] took visual tracking uncertainties into consideration and showed robustness in the presence of outliers and occlusions for cable manipulation.

Inspired by the existing work and our previous research study on the model-based linear flexible object manipulation [28,29], we have the following contributions in this paper: (1) It introduces a novel 3D geometrical model of the linear flexible objects subject to gravity based on 2D models on two projection planes and learned object positions; (2) it presents an autonomous system framework for accomplishing the DRC Plug Task; (3) it introduces a novel strategy, Simulation-to-Real-to-Simulation (Sim2Real2Sim), for bridging the gap between simulation and real world; (4) it presents a physical model of the linear flexible objects and an identification approach of getting the model parameters. It should be noted that DRC Plug Task has been selected as the validation study in this research for two reasons: (i) Our team participated in the DRC Finals [14], and (ii) The DRC Plug Task provides a well-defined set of requirements to generalize the approach presented here to other applications.

This paper is organized as follows. In Section 2, we introduce the methodology of automating the DRC Plug Task in the real world. An automation system framework, a reliable geometrical model of the linear flexible objects, and a robust pose alignment controller are covered in this section. Section 3 describes how we complete the DRC Plug Task in simulation. In this section, we introduce how we build a complete simulation environment, a novel Sim2Real2Sim strategy, and a physical model of the linear flexible objects with identified parameters. Section 4 shows the experimental results. Section 5 includes the conclusion and directions for future work.

2. Task Automation in Real World Based on Our Geometrical Modeling of the Linear Flexible Objects

The DRC Plug Task involves unplugging a power cable from one socket and plugging it into another. The problem involves detecting a flexible cable in a massive environment, grasping the cable with a feasible grasp pose, controlling the shape of the cable to fit the target pose, and inserting the cable tip to the target socket.

2.1. Benchmarking Setup and Flow of Our Designed System

A benchmarking setup is used in this study shown in Figure 1. The setup consists of a power cable (Deka Wire DW04914-1) with a plug (Optronics A7WCB) on the tip, two power sockets (McMaster-Carr 7905k35) attached on the wall, two neodymium disk magnets (DIYMAG HLMAG03) glued to the socket and the plug to provide a suction force [12]. Our system also includes a 6-DOF JACO v2 arm with three fingers, and two RGB-D cameras, i.e., Realsense D435 and Microsoft Kinect. The Realsense camera is used to estimate and model the shape of the cable, while the Kinect camera is used to estimate the socket pose and filter the point cloud. This scenario corresponds to a humanoid robot equipped with a depth camera at its left arm wrist and another depth camera at its head. One arm performing the task while the other is providing the perception feedback.

From a system design perspective, to complete the DRC Plug Task autonomously, the system needs to be able to model a linear flexible object, to keep track of the cable configurations and to detect the pose of the target socket through pose estimation, to plan motions for the robot, and to align the cable tip with the target socket by a controller. A flow of the proposed system architecture is presented in Figure 2.

We can divide the entire process into five phases: Initialize, Grasp, Unplug, Pre-insert, and Insert. Our goal is to give the robot the ability to automatically switch the phases. From the Initialize phase to the Grasp phase, the system needs to sense the environment, including estimating the socket pose, filtering the point cloud, and modeling the cable. After getting the model of the cable, the system will estimate the poses of the cable and the cable tip and select the grasp point on the cable. Then the robot will go and grasp the cable. From the Unplug phase to the Pre-insert phase, the robot needs to align the pose of the cable tip with the target pose. From the Grasp phase to the Unplug phase, a planned motion away from the target socket and perpendicular to the target socket hole plane is executed by the robot, and another planned motion perpendicular to the socket hole plane but close to the socket is executed by the robot to transfer from the Pre-insert phase to the Insert phase. To automate the DRC Plug Task, the robot needs to know where the target is. We used the Hough Circle Transform [30] to detect the hole (circle) of the target socket. Then the coordinates of the circle center can be obtained. By integrating the depth information at the circle center, we can get the 3D position of the target socket center. Random sample consensus (RANSAC) method [31] is applied to detect the wall plane which is parallel to the socket hole plane. The normal to the wall plane and the 3D position of the target center are used to define the target frame (named “Target_Socket”).

2.2. Geometrical Modeling of the Linear Flexible Objects

It is challenging to find specific irregular objects in a noisy environment. In this task, a real-time object detection method called YOLO [32] is used for detecting the flexible power cable in the 2D images (Figure 3(left)). The process requires training a convolutional neural network (CNN) with self-labeled images. We trained the network for 40,000 steps with 100 labeled images. The output of YOLO is a bounding box that narrows the region down to a search for the cable. To get the pixels corresponding to the object, we then use the color information and detect the black pixels in this region (Figure 3(middle)). These pixels are stored in a set in the order from top to bottom and left to right, and we define the middle pixel as the center pixel of the object which is marked as “Center” in Figure 3(left). By integrating the depth information, we can get the 3D positions of the object center. A PassThrough filter from the point cloud library (pcl) [33] was used to filter the point cloud from three dimensions as shown in Figure 3(right). The point cloud in the narrowed bounding box served to model the cable. It is published through ROS at a frequency of 30 Hz.

In Figure 3 (right), the remaining points in the filtered point cloud are shown in white with respect to the world frame. The x, y, and z axes of the world frame are represented by the red, green, and blue bars, respectively. Modeling the cable in 3D space is still a research problem in linear flexible object manipulation. On the other hand, modeling a curved line in 2D space is a solved problem. We can exploit this by projecting the 3D curve onto 2D spaces. In this study, we project the 3D point cloud onto the y-x and y-z planes. However, the projection in 2D may have multiple corresponding x or z values to the y coordinate. For example, if the cable has a “⊃” shape, in order to get a unique projection model, we filtered out the point cloud below the rightmost point. Moreover, we assume that the cable is not in more complex shapes, e.g., “S” or “$\alpha$” shape. Due to the relatively high stiffness and large bending radius of the power cables, we found that a quadratic polynomial equation is sufficient to represent their shapes:

$$x=a_0+a_{1}y+a_2{y}^2,$$

(1)

$$z=b_0+b_{1}y+b_2{y}^2.$$

(2)

The polynomial coefficients $a_0$, $a_1$, $a_2$, $b_0$, $b_1$, and $b_2$ are estimated by using the least squares method, and they continuously change based on the shape of the power cable. In order to model the cable in 3D, we uniformly sample the points in Figure 3(right) along the y-axis. The corresponding x and z values can be calculated by using Equations (1) and (2). This projection method is efficient, and the model can be published through ROS at a frequency of 29.997 Hz.

Figure 4 shows an example of modeling result with 10 sample points. The power cable is visualized in Rviz by markers. Green markers represent the sampling points, consecutive points are connected by blue lines, and vertical upward red lines are used to illustrate the displacements between the points.

In order for the robot to detect and track the location of the cable and cable tip, we propose a piece-wise linear model for the object. In Figure 4, we denote the leftmost point as $p_1$ and the remaining points as $p_2,p_3,\dots ,p_N$ from left to right, where N is the number of sample points. We denote the leftmost blue line that connects $p_1$ and $p_2$ as $l_1$ and rest of the blue lines as $l_2,l_3,\dots ,l_{N-1}$. We assume that the line $l_i$ is the tangent at the point $p_i$. Because the cable tip has the round shape, its roll angle can be neglected (we set it to 0). After setting the point $p_1$ as the origin and defining the x axis to the tangent of the point $p_1$, the “cable_tip” frame is defined in Figure 5(left).

Now that the positions of all the sample points and the pose of the cable tip are known with respect to the robot frame, a feasible grasp pose for the robot can be calculated to grasp the cable. Because the plug is inside the socket, we consider only the sampling points on the cable except the cable tip as the grasp point candidates. To select the grasp point on the cable, the following trade-off needs to be taken into account. As the grasp point gets closer to the cable tip, the risk of the robot colliding with the socket or the wall increases and the number of points in the filtered point cloud reduces. On the other hand, when the grasp point is too far away from the cable tip, the robot can not align the cable tip pose with the target socket pose because the cable will dangle substantially. Thus, the grasp point needs to be selected empirically to meet these two constraints. For this purpose, we pre-define a minimum distance from the grasp point to the cable tip ($p_1$) along the cable as $d_{min}$, and a maximum distance as $d_{max}$. $d_{min}$ and $d_{max}$ are pre-defined with the robot holding the cable before the experiment. With this step, the cable deformation characteristics can be ignored for selecting the grasp point, and this vision-only method is satisfied for selecting a feasible grasp point. Since we can calculate the length of the lines $l_1,\dots ,l_{N-1}$, we can also calculate the distance $d_s$ from each sampling point to the cable tip by using:

$$d_s\triangleq \sum _{i=1}^s{l}_i,s=1,\dots ,N-1.$$

(3)

The grasp point needs to be selected such that $d_s\in [d_{min},d_{max}]$ for the robot to be able to align the cable tip pose with the target pose. By using the same method of estimating the “cable_tip” frame, we can get the “cable” frame (Figure 5(right)), which is used to get the grasp pose of the robot’s end-effector. The grasp pose is then used as the target of the motion planner. We implemented both MoveIt! [34] and TrajOpt [35] for the motion planing in this task.

Once grasping of the cable is completed, a pulling motion along the y axis of the “world” frame is generated to unplug the cable. Figure 6a shows the “cable_tip” frame after the robot unplugs the power cable. We filter out the point cloud of the plug during modeling which improves the accuracy of the model because the plug is a straight and rigid object.

2.3. Pose Alignment Controller

In order to finish the insertion task, the robot needs to adjust the pose of the cable tip to match with the pose of the target socket. A pre-insert frame right in front of the target socket is defined as our target pose for the pose alignment controller. Because of the continuous visual feedback, we decide to use the visual servoing approach to align the cable tip pose with the target socket pose.

To explain this algorithm in a succinct way, we denote the transformation matrix from the “cable_tip” frame to the pre-insert frame as ${\mathbf{T}}_{ct}^{pre}$, the transformation matrix from the pre-insert frame to the “end-effector” frame as ${\mathbf{T}}_{pre}^{ee}$, the transformation matrix from the “cable_tip” frame to the “end-effector” frame as ${\mathbf{T}}_{ct}^{ee}$.

${[\Delta x\Delta y\Delta z\Delta \alpha \Delta \beta \Delta \gamma ]}^{\top }$ are the translational (x, y, z) and rotational ($roll$, $pitch$, $yaw$) deviations between the “cable_tip” frame and the pre-insert frame under the “end-effector” frame which can be calculated as

$$\left[\begin{array}{c}\Delta x\\ \Delta y\\ \Delta z\end{array}\right]=\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]-\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right],\left[\begin{array}{c}\Delta \alpha \\ \Delta \beta \\ \Delta \gamma \end{array}\right]=\left[\begin{array}{c}{\alpha }_1\\ {\beta }_1\\ {\gamma }_1\end{array}\right]-\left[\begin{array}{c}{\alpha }_2\\ {\beta }_2\\ {\gamma }_2\end{array}\right],$$

(4)

where $x_1$, $y_1$, $z_1$, ${\alpha }_1$, ${\beta }_1$, and ${\gamma }_1$ are translational and rotational terms from the transformation ${\mathbf{T}}_{pre}^{ee}$ under the “end-effector” frame, $x_2$, $y_2$, $z_2$, ${\alpha }_2$, ${\beta }_2$, and ${\gamma }_2$ are translational and rotational terms from the transformation ${\mathbf{T}}_{ct}^{ee}$ under the “end-effector” frame.

Then the linear and angular velocities under the “end-effector” frame can be calculated [36]:

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{z}\end{array}\right]=\left[\begin{array}{c}\Delta x\\ \Delta y\\ \Delta z\end{array}\right]\frac{1}{\Delta t},$$

(5)

$$\left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]=\left[\begin{array}{ccc}1& 0& -sin(\Delta \beta )\\ 0& cos(\Delta \alpha )& cos(\Delta \beta )sin(\Delta \alpha )\\ 0& -sin(\Delta \alpha )& cos(\Delta \beta )cos(\Delta \alpha )\end{array}\right]\left[\begin{array}{c}\Delta \alpha \\ \Delta \beta \\ \Delta \gamma \end{array}\right]\frac{1}{\Delta t},$$

(6)

where $\Delta t$ is the execution time, $\dot{x},\dot{y},\dot{z},w_x,w_y,and$ $w_z$ are linear and angular velocities under the “end-effector” frame, which will be used as inputs for our PD controller.

A PD controller was designed to calculate the Cartesian velocity ($\dot{\mathbf{x}}$) for the robot’s end-effector:

$$\dot{\mathbf{x}}=K_p\mathbf{e}+K_d\dot{\mathbf{e}},$$

(7)

where $\mathbf{e}$ is the error, which is ${\left[\dot{x}\dot{y}\dot{z}{w}_x{w}_y{w}_z\right]}^{\top }$, $K_p$ and $K_d$ are proportional and derivative terms. Our designed values for $K_p$ and $K_d$ are 2.0 and 0.2.

After getting the Cartesian velocity from our PD controller, a velocity controller built in the Jaco robotic arm generates the controlled joint velocity ($\dot{\mathbf{q}}$) by using robot Jacobian matrix ($\mathbf{J}$):

$$\dot{\mathbf{q}}={\mathbf{J}}^{-1}\dot{\mathbf{x}}.$$

(8)

Algorithm 1 shows the pose alignment control loop. The control loop keeps running until the translational deviations $(\Delta x_e$, $\Delta y_e$, $\Delta z_e)$ and the rotational deviations $(\Delta {\alpha }_e$, $\Delta {\beta }_e$, $\Delta {\gamma }_e)$ from the “cable_tip” frame to the pre-insert frame (${\mathbf{T}}_{ct}^{pre}$) satisfy the thresholds which are ${ϵ}_1$ (m) and ${ϵ}_2$ ($rad$). A successful example of the robot aligning the cable to pre-insert frame is shown in Figure 6b. For the final insertion step, a translation along the x axis of the “cable_tip” frame is applied. The insertion is facilitated by the magnets on the plug and in the socket.

Algorithm 1: Pose alignment algorithm

3. Task Automation in Simulation Based on Our Physical Model of the Linear Flexible Objects

Simulation plays a very important role in robotics [37]. Various simulators (e.g., Gazebo, OpenRAVE, MuJoCo) are used to design the robot model, create different simulation environments, analyze the kinematics and dynamics of the robot, design different plan or control algorithms, investigate the performances of the system, etc. The construction of a real environment is usually more expensive than building a simulation environment. In simulation, we can build objects with their geometric and physical properties. We can also design new robot models or import existing robot models to complete a series of manipulation tasks. Usually, the robot needs to make physical contact with the environment and objects when completing the manipulation tasks. Learning to use the robot to manipulate objects in simulation can help with avoiding damage to the robot, the environment, and humans.

The DARPA Robotics Challenge (DRC) is a competition funded by the US Defense Advanced Research Projects Agency between 2012 and 2015. The DARPA Robotics Challenge (DRC) Simulator (DRCSim) and the Space Robotics Challenge Simulation (SRCSim) have related open-source simulations for most tasks in the competition. However, the Plug Task used in the 2015 DARPA Robotics Challenge Finals does not have a complete simulation environment. Based on our group’s prior experience in this competition [14] and the detailed definition and requirements of this task, we decided to build a complete simulation environment and complete this task in simulation for our research study.

3.1. Simulation Environment Setup

The simulation setup consists of a wall, two power sockets, a power cable with a plug, a Kinova Jaco V2 robotic arm, and a Kinect camera. Figure 7 shows the simulation setup in the Gazebo simulator [38]. Modeling of rigid objects in the simulator is a solved problem: The wall can be modeled as a box and the plug can be fitted as a cylinder. Modeling objects of irregular shape is relatively difficult, but we can get the model in a solid modeling computer-aided design tool (e.g., SolidWorks) and then use a converter (e.g., SolidWorks to URDF Exporter) to get the model in the desired format. The RGB-D camera in the simulated environment is a Kinect camera. To read the point cloud information, we need to model the Kinect camera as a Depth Camera Plugin in Gazebo. The simulated Jaco arm is cloned from the official Kinova GitHub website. In simulation, we assume that the positions of the sockets are known. Modeling of the cable in the simulator is a challenge and it will be introduced in the next subsection.

3.2. Physical Model of the Linear Flexible Objects

Inspired by the “fire hose” model in Gazebo and the Piecewise Constant Curvature (PCC) model [39], a Dynamically-Consistent Augmented Formulation [40] of the PCC cable model is created in Figure 8a. A Gazebo model of the cable is also built based on the PCC model (see Figure 8b). Fifteen links are used to model the cable, each link is 5 cm long and weighs 50 g with the center of mass in the middle of the arc. The plug is also a link with length 10 cm and weight 100 g. These links are connected by revolute joints (except the joint between the plug and link-1) with the roll and pitch rotations. Each joint has stiffness and damping properties. There exists a trade-off in selecting the number of links for the simulated cable. In theory, more links of the cable model will produce more accurate simulation results. On the other hand, more links will cause the computing speed to slow down or even the simulator to crash. We observed that if the number of links is increased to twenty, sometimes there exists a misalignment between the links.

3.3. Sim2Real2Sim—Achieving Flexible Object Manipulation in Simulation with Identified Physical Model

The gap between simulation and real world was introduced in Simulation-to-Real (Sim2Real) [41,42,43,44]. Simulation is observed to have inevitable simplifications with heavy optimization. Besides, there exist physical events not modeled in simulators and parameters of the simulated models that need to be identified. Therefore, policies or parameters learned [45] and controllers designed [43] in simulation can not be transferred to the real world directly.

To reduce the gap between simulation and real world especially in flexible object manipulation, we propose a new strategy called Simulation-to-Real-to-Simulation (Sim2Real2Sim). Figure 9 shows the flowchart of the Sim2Real2Sim strategy we will use for bridging the gap. We start with a rough simulation environment with the estimated models. Then we test the system framework in the real world based on the methods developed in simulation and collect the data from the real world. Finally, we go back to the simulation and update the models and methods based on the data from the real world.

The research approach from simulation to real world implementation is widely applied in humanoid robot [46], mobile robot [47], robotic arm [48], etc. Normally, we complete a task in simulation first, then transfer the methods used in simulation to the real world. This step is named “simulation to real world” in the Sim2Real2Sim strategy. Building a simulation environment with all rigid objects is well-developed and these objects have nearly the same behavior in simulation and real world. However, simulation of soft objects usually simplifies the model; the gap between simulation and real world can not be ignored. Our proposed Sim2Real2Sim approach is for solving this issue. More specifically, we use real world data to improve the simulated models and we call this step “real world to simulation”. Therefore, our idea of Sim2Real2Sim is like a research strategy and a generalized methodology to reduce the gap between simulation and the real world and to obtain a high-fidelity simulation environment for flexible object manipulation.

3.3.1. Simulation to Real World

Led by the Sim2Real2Sim strategy, we build an initial simulation environment based on the task requirements (see Figure 7). Recall that the DRC Plug Task can be divided into five phases (Initialize, Grasp, Unplug, Pre-insert, and Insert). In the Initialize and Grasp phases, we use the same geometrical model of the cable mentioned in Section 2.2 to estimate the cable tip pose and select the grasp point. After completing the Grasp of the cable in simulation with 30 successful experiments, we apply the same motion planning method to the real robot, and as expected, the real robot can Grasp the cable at the grasp point autonomously.

Unplugging the cable from the socket is straightforward with a backward panning motion for the end-effector. After the cable being plugged out of the socket, the weight of the front section (plug) of the cable will cause the cable to dangle. In our model (see Figure 8), this deformation depends on the joint stiffness and damping. We observe that the deformation of the cable is different by randomly setting the values for the joint stiffness and damping. Two special cases are: (1) The cable dangles too much (see Figure 10a); (2) the cable barely dangles (see Figure 10b). However, our geometric model works well in both cases (see Figure 11). To get reasonable values for the joint stiffness and damping and to narrow the gap between simulation and real world, we need to collect data from the real world and go back to simulation to update the models and methods. To automate the Plug Task, we need to figure out how to carry the cable to the Pre-insert position after the cable being Unplugged from the original socket. We use the visual servoing approach mentioned in Section 2.3 to reduce the differences between the “cable_tip” frame and the Pre-insert frame. After the cable tip moves to the Pre-insert position, a forward panning motion for the end-effector will Insert the cable tip (plug) to the target socket.

3.3.2. Real World to Simulation

After implementing the methods developed in simulation to real world, we realized that the deformation of the cable in simulation and real world is different after the robot unplugs the cable from the socket, which means our physical model in simulation needs to be tuned to match with the real world.

As we mentioned before, the cable is modeled as a rigid manipulator with passive revolute joints (see Figure 8). The dynamics of the cable can be represented as:

$$M\ddot{\mathbf{q}}+C\dot{\mathbf{q}}+G+J^T{\mathbf{f}}_{ext}+K\mathbf{q}+D\dot{\mathbf{q}}=\mathbf{\tau },$$

(9)

where $\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}$ represent joint position, velocity, and acceleration. M is the inertia matrix. C is the centrifugal and Coriolis forces matrix. G is gravitational forces or torques. $J^T$ is the transpose of the robot Jacobian. ${\mathbf{f}}_{ext}$ is the external force. K is the stiffness and D is the damping. $\mathbf{\tau }$ is a vector of joint torques, corresponding to the torques and forces applied by the actuators at the joints. Our goal is to estimate the stiffness and damping in this equation.

Recalling that each joint of the cable has roll and pitch rotations (see Figure 8), we simplify the model by only considering the pitch. Our first challenge is to get the joint values. AprilTag [49] is used for getting the joint positions. Figure 12 shows the start and end points of recording the joint positions with a 100 g weight added to the tip. Four apriltags with side lengths of 2.0 cm [50] are attached on the cable 5 cm apart (same as the link length in Gazebo). The joint position $\mathbf{q}$ and the time stamp are recorded for calculating joint velocity $\dot{\mathbf{q}}$ and acceleration $\ddot{\mathbf{q}}$. After we get joint values, the robot Jacobian J can be calculated. Because the joints on the cable are passive, $\mathbf{\tau }$ equals to $\mathbf{0}$. The external force ${\mathbf{f}}_{ext}$ is also known by multiplying the mass of the weight (0.1 kg) with the gravitational acceleration (9.8 m/s${}^2$).

Computing of the left three terms ($M\ddot{\mathbf{q}}+C\dot{\mathbf{q}}+G$) in Equation (9) is our second challenge. We refer to the Recursive Newton Euler (RNE) approach [51,52,53]. The RNE approach includes two recursions: Forward recursion for computing velocities and accelerations, and backward recursion for computing forces and torques. Figure 13 shows the parameters of three adjacent links and Algorithm 2 shows the detailed RNE algorithm. The RNE algorithm gives us the torque required for each joint to overcome the gravity of the model and the force generated from the motion if the cable joints have no friction or damping and there is no external force. The left three terms ($M\ddot{\mathbf{q}}+C\dot{\mathbf{q}}+G$) in Equation (9) can be represented as:

$$M\ddot{\mathbf{q}}+C\dot{\mathbf{q}}+G={\mathbf{\tau }}_{RNE},$$

(10)

where ${\mathbf{\tau }}_{RNE}$ equals to ${[{\tau }_1,{\tau }_2,\dots {\tau }_n]}^T$ ($n=4$) calculated from the RNE method.

Algorithm 2: Recursive Newton Euler algorithm

Forward recursion:{ Initial: Velocity and acceleration of the base link both equal to 0 Compute link angular velocity: $w_i={\left(R_i^{i-1}\right)}^T(w_{i-1}+{\dot{q}}_i{z}_{i-1})$ Compute link angular acceleration: ${\dot{w}}_i={\left(R_i^{i-1}\right)}^T({\dot{w}}_{i-1}+{\ddot{q}}_i{z}_{i-1}+{\dot{q}}_i{w}_{i-1}\times z_{i-1})$ Compute linear acceleration of origin of frame i:  $a_i={\left(R_i^{i-1}\right)}^T{a}_{i-1}+{\dot{w}}_i\times L_{i-1,i}+w_i\times (w_i\times L_{i-1,i})$ Compute linear acceleration of centre of $C_i$: $a_{ci}=a_i+{\dot{w}}_i\times L_{i,c_i}+w_i\times (w_i\times L_{i,c_i})$} Backward recursion:{ Compute force exerted by link $i-1$ on link i: $f_i=R_{i+1}^i{f}_{i+1}+m_i{a}_{ci}$ Compute moment exerted by link $i-1$ on link i:  ${\mu }_i=-f_i\times (L_{i-1,i}+L_{i,c_i})+{\mu }_{i+1}+f_{i+1}\times L_{i,c_i}+I_i{\dot{w}}_i+w_i\times \left(I_i{w}_i\right)$ Compute torque exerted by link $i-1$ on link i: ${\tau }_i={\mu }_i^T{z}_{i-1}+{\tau }_{inertia}$}

OpenRAVE [54] is used for computing the robot Jacobian J and the inverse dynamic terms $M\ddot{\mathbf{q}}+C\dot{\mathbf{q}}+G$. Figure 14 shows the physical model built in OpenRAVE. We pick the first four links of the cable for modeling because the cable tip pose is not guaranteed to be aligned with the socket pose if the grasp point is beyond this range. The robot Jacobian and inverse dynamics are computed based on this model.

At this point, we can rewrite Equation (9) as:

$$K\mathbf{q}+D\dot{\mathbf{q}}=\mathbf{\tau }-(M\ddot{\mathbf{q}}+C\dot{\mathbf{q}}+G+J^T{\mathbf{f}}_{ext}).$$

(11)

The terms to the right of the Equation (11) are known. To get the stiffness K, we use the robot configuration when the joint velocity $\dot{\mathbf{q}}$ equals to $\mathbf{0}$. In our case, we use the joint values when the cable is finally stationary after adding the weight (see Figure 12b). The stiffness K can be computed by using Equation (12) with $\dot{\mathbf{q}}=\mathbf{0}$,

$$K=(\mathbf{\tau }-(M\ddot{\mathbf{q}}+C\dot{\mathbf{q}}+G+J^T{\mathbf{f}}_{ext}+D\dot{\mathbf{q}})){\mathbf{q}}^{+},$$

(12)

where ${\mathbf{q}}^{+}$ is the pseudoinverse of $\mathbf{q}$.

After getting stiffness K, we take it to Equation (11). The damping D can then be calculated as:

$$D=(\mathbf{\tau }-(M\ddot{\mathbf{q}}+C\dot{\mathbf{q}}+G+J^T{\mathbf{f}}_{ext}+K\mathbf{q})){\dot{\mathbf{q}}}^{+},$$

(13)

where ${\dot{\mathbf{q}}}^{+}$ is the pseudo inverse of $\dot{\mathbf{q}}$. Unlike the calculation for stiffness, we use joint values prior to the cable entering its final stationary position to ensure that joint velocity is not $\mathbf{0}$.

Finally, we update the Gazebo model based on the calculated stiffness and damping. Recalling that each joint on the cable also has the roll rotation. We can use the same method to identify the stiffness and damping properties if we can figure out how to add a constant twist torque on the cable tip. This part is left as future work. Currently, we use the joint limits to set the roll rotation based on the observation. Figure 15 shows the deformation of the cable with the updated model parameters in the simulation. Detailed comparison of the model deformation in simulation with the cable deformation in the real world will be discussed in Section 4.

4. Experiments and Results

Our system framework for task automation is evaluated by running the DRC Plug Task both in simulation and real world for 20 trials. In simulation, the average completion time is 66.25 s with an average real-time factor: 0.65. Figure 16 shows the robot completing the DRC Plug Task in simulation. In real world, the average completion time is 31.53 s. Figure 17 shows the robot completing the DRC Plug Task in the real world. We also test our automation approach with a different robot platform which is a Kinova Gen3 7-DOF arm. Figure 18 shows the Gen3 robot arm completing the DRC Plug Task in the real world. In order to validate the performance of our geometrical modeling of the cable, pose alignment controller, and the cable model in simulation, we conduct different experiments in each aspect.

4.1. Validation of the Geometrical Modeling of the Cable and the Pose Alignment Controller

To validate the performance of our modeling method and pose alignment controller, we implement three experiments. The first experiment is for testing the grasp functionality with different initial poses of the cable. The second experiment shows the performance of the pose alignment controller with two different cables. The last experiment shows the robustness of our pose alignment controller under external disturbances.

The first experiment is for testing the grasp functionality with different initial poses of the cable modeled by our geometrical modeling method. Recalling the trade-off mentioned in Section 2.2, we need to pick values of $d_{min}$ and $d_{max}$ for different cables. For the power cable (with plug), we define $d_{min}$ = 18 cm and $d_{max}$ = 30 cm. For the HDMI cable, these parameters are selected as $d_{min}$ = 12 cm and $d_{max}$ = 24 cm. Our method will prioritize the sample point closest to the midpoint in this range as grasp point when performing tasks. Figure 19 shows the robot is able to grasp the cable with three different initial states. Figure 20 shows the modeling of the cable for these three different initial poses.

Table 1. Estimated model parameters of the power cable for three different initial states.

Poses	${\mathit{a}}_0$	${\mathit{a}}_1$	${\mathit{a}}_2$	${\mathit{b}}_0$	${\mathit{b}}_1$	${\mathit{b}}_2$
left	−1.882	−7.295	−5.773	−6.041	−20.004	−15.528
middle	−0.385	−2.670	−2.191	−1.917	−6.981	−5.243
right	0.006	−1.377	−1.125	−0.979	−3.955	−2.806

shows the model parameters for these states.

The second experiment shows the performance of the pose alignment controller with two different cables. Due to the limitation of point cloud publishing frequency (30 Hz), we set the constraints for linear and angular velocities in the Cartesian space as 1.5 m/s and 0.6 rad/s to prevent the substantial deformation of the cable in the interval of the point cloud update. We implement the pose alignment controller with two different cables (10 trials/each). Figure 21 shows two successful runs during these experiments. Figure 22 shows the pose differences between the cable tip frame and the pre-insert frame. Our pose adjustment control loop ends when the pose differences meet the thresholds which is 0.01 m for translational terms and 0.02 rad for rotational terms. In this experiment, we conclude that the HDMI cable (average elapsed time: 12.26 s) needs more time than the power cable (average elapsed time: 5.59 s) to adjust the pose. A potential reason might be that the HDMI cable has more deformation than the power cable.

Another experiment is performed for evaluating the robustness of the pose alignment controller by adding disturbances to the pose alignment control loop and testing whether our algorithm can still achieve the target. Figure 23 shows that different disturbances added to the cable tip while the system is in the pose alignment loop and the final pose of the cable after the loop, which demonstrates that our pose alignment controller is able to resist such external disturbances.

These results demonstrate that our method for modeling linear flexible objects can provide a 3D model with adaptive parameters based on the visual feedback. This model is capable of representing various shapes of the cable during the Plug Task. More importantly, our pose alignment algorithm is robust to handle different cable-like objects and is capable of resisting disturbances.

4.2. Validation of the Cable Model in Simulation

To validate the model in simulation, we designed two experiments. First, we fix the same link on the cable to the world, and compare the joint positions of the cable in simulation and real world. Second, we fix different links on the cable horizontally, and compare the pose of the cable tip in simulation and real world. In simulation, the link of the simulated cable can be fixed to the world by adding a “fixed” joint connecting the “world” link and the link you want to fix. In real world, we fix the cable by using a zip-tie. Figure 24 shows the setup in simulation and real world for comparison of the cable deformation. Apriltags are attached to the cable tip and on the first four revolute joints to get the transformation at each joint. Figure 25 shows the visualized frames in Rviz, and the “tag_1” frame is equivalent to the “cable_tip” frame.

Our first experiment is comparing the deformation of the cable by fixing the same link horizontally to the world. We use the setup in Figure 24 and obtain the frames (“tag_1”, “tag_2”, …, “tag_5”) both in simulation and real world (see Figure 25). Transformations of these frames are used to get the joint positions for the comparison in simulation and real world. We first compare the joint positions of the cable with no external force, then we add different weights to the cable tip and compare the differences of the joint positions under these disturbances.

Figure 26 shows the comparison result by fixing the link-5 (the link after tag-5) on the cable to the world. From the Figure 26, we conclude that the shape of the cable in the simulation is close to the real cable with the largest difference of the joint positions being 0.005 rad, and the largest percent error between the simulation and the real world is 2.00%. To test the simulation model with external disturbances, we add two different weights (50 g and 100 g) to the cable tip, and we compare the joint positions of the simulated cable with the one of the real cable (see Figure 27 and Figure 28). The percent errors are reasonable which proves the accuracy of our simulated cable model.

Our second experiment is fixing different links on the cable horizontally to the world in simulation and real world to compare the deformation of the cable tip. We compare the rotation angles around the z-axis from the frame we have fixed to the world to frame “tag_1”. This experiment is to compare the sagging angle of the cable tip in simulation and real world. Similar to the first experiment, three comparisons are made for validation.

Figure 29 shows the comparison result by fixing different links on the cable to the world. From the Figure 29, we conclude that the difference of the sagging angle between the simulation model and real cable is within a reasonable range (<0.04 rad), and the largest percent error is 3.38%.

We also run this experiment with two different weights (50 g and 100 g) added to the cable tip. Figure 30 and Figure 31 show the results. In the case of a 50 g weight added to the cable tip, the maximum difference of the cable sagging angle between the simulation and the real world is 0.02 rad, and the largest percent error is 3.12%. If a 100 g weight is added to the cable tip, the maximum difference of the cable sagging angle between the simulation and the real world is 0.048 rad, and the largest percent error is 4.11%.

5. Conclusions

In this paper, we proposed a geometrical modeling method based on the curves on two projection planes for linear flexible objects subject to gravity. The geometrical model enables tracking the 3D curvature of the linear flexible object, the pose of the tip and the pose of the selected grasp point on the object. A robust pose alignment controller based on the geometrical model with adaptive parameters can bring the cable tip to a desired pre-insert position. We designed and used an autonomous system pipeline with our formulated methods to accomplish the DRC Plug Task autonomously.

We also proposed a new strategy called Sim2Real2Sim. This strategy includes two procedures: sim-to-real and real-to-sim. The sim-to-real procedure is to organize the methods used in simulation and apply them to the real world in a reasonable way. The real-to-sim procedure is for updating the models in simulation which bridges the gap between simulation and real world. A novel identification method based on inverse dynamics is implemented to update the parameters of the simulated flexible objects. The comparison of the deformation of the flexible objects in simulation and real world proves the accuracy of the model in the simulation

The success of the system framework running both in simulation and the real world demonstrates the practicability and reliability of our proposed methods. Possible future work includes: (1) Explore different model representations of the cable shapes in the case of more complex deformations; (2) apply our approach to different robot platforms (e.g., a dual-arm system) or to other flexible object manipulation tasks (e.g., a cable routing assembly task); (3) get more evaluations on our Sim2Real2Sim strategy; (4) update the simulation model for the linear flexible objects by adding more degrees of freedom (e.g., using more apriltags); (5) bring the material and friction information from the real world to the simulation to obtain a more close-to-reality simulated cable model and a better simulation when the cable makes contact with the environment.