Principles of robotics, programming a da Vinci surgical robot in a simulated environment, ROS1-ROS2 bridge

Rigid body motion

Def. Rigid body

A rigid body is defined as a body on which the distance between two points remains constant in time regardless of the force applied on it.

Shape and the volume of the rigid bodies are also constant.
The pose of a rigid body can be given by the three coordinates of three of its points that do not lie on the same straight line.

The pose of a rigid body can be described in a more expressive way by the three coordinates of one of its points chosen arbitrarily position and the body's orientation.
The motion of rigid bodies is composed by two elemental motions: translation and rotation.
During translation, all points of the body move along straight, parallel lines.

During rotation, the position of the points of the rotational axis are constant, and the other points of the body move along circles in planes perpendicular to the axis of rotation.
The free motion of rigid bodies can always be expressed as the superposition of a translational motion and a rotation around a single axis.

3D transformations

Position: 3D offset vector
Orientation: 3 x 3 rotation matrix
- further orientation representations: Euler-angles, RPY, angle axis, quaternion
Pose: 4 × 4 (homogenous) transformation matrix
Frame: origin, 3 axes, 3 base vectors, right hand rule
Homogenous transformation: rotation and translation in one transfromation
- e.g., for the rotation \(\mathbf{R}\) and translation \(\mathbf{v}\):

\[ \mathbf{T} = \left[\matrix{\mathbf{R} & \mathbf{v}\\\mathbf{0} & 1 }\right] = \left[\matrix{r_{1,1} & r_{1,2} & r_{1,3} & v_x\\r_{2,1} & r_{2,2} & r_{2,3} & v_y\\r_{3,1} & r_{3,2} & r_{3,3} & v_z\\\ 0 & 0 & 0 & 1 }\right] \]

Homogenous coordinates:
- Vector: extended with 0, \(\mathbf{a_H}=\left[\matrix{\mathbf{a} \\ 0}\right]=\left[\matrix{a_x \\ a_y \\ a_z \\ 0}\right]\)
- Point: extended by 1, \(\mathbf{p_H}=\left[\matrix{\mathbf{p} \\ 1}\right]=\left[\matrix{p_x \\ p_y \\ p_z \\ 1}\right]\)
- Applying transfomrations is much easier:

\[ \mathbf{q} = \mathbf{R}\mathbf{p} + \mathbf{v} \to \left[\matrix{\mathbf{q} \\ 1}\right] = \left[\matrix{\mathbf{R} & \mathbf{v}\\\mathbf{0} & 1 }\right]\left[\matrix{\mathbf{p} \\ 1}\right] \]

Degrees of Freedom (DoF): the number of independent parameters.

Principles of robotics

Robots are built of: segments (or links) és joints
Task space (or cartesian space):
- 3D space around us, where the task, endpoint trajectories, obstacles are defined.
- TCP (Tool Center Point): Frame fixed to the end effector of the robot.
- Base frame, world frame
Joint space:
- Properties or values regarding the joints.
- Low-level controller.
- Joint angles, joint velocities, accelerations, torques....

Python libraries

Numpy

Python library
High dimension arrays and matrices
Mathematical functions

import numpy as np

# Creating ndarrays
a = np.zeros(3)
a.shape
a.shape=(3,1)
a = np.ones(5)
a = np.empty(10)
l = np.linspace(5, 10, 6)
r = np.array([1,2])    # ndarray from python list
r = np.array([[1,2],[3,4]])
type(r)

# Indexing
l[0]
l[0:2]
l[-1]
r[:,0]

# Operations on ndarrays
r_sin = np.sin(r)
np.max(r)
np.min(r)
np.sum(r)
np.mean(r)
np.std(r)

l < 7
l[l < 7]
np.where(l < 7)

p = np.linspace(1, 5, 6)
q = np.linspace(10, 14, 6)

s = p + q
s = p * q
s = p * 10
s = p + 10
s = p @ q    # dot product
s = r.T

If not installed:

pip3 install numpy

Matplotlib

Visualization in python
Syntax similar to Matlab

import numpy as np
from matplotlib import pyplot as plt

X = np.linspace(-np.pi, np.pi, 256)
C, S = np.cos(X), np.sin(X)

plt.plot(X, C)
plt.plot(X, S)

plt.show()

If not installed:

pip3 install matplotlib

Practice

1: ROS1-ROS2 bridge install

Open the ~/.bashrc file and comment out the lines responsible for source-coding ROS 1, ROS 2 and additional ROS workspaces.

Add the following line to the ~/.bashrc file:

export ROS_MASTER_URI=http://localhost:11311

Install the ros-foxy-ros1-bridge package:

sudo apt update
sudo apt install ros-foxy-ros1-bridge

2: Catkin workspace

Install the catkin build tools package:

sudo apt update
sudo apt-get install python3-catkin-tools python3-osrf-pycommon

Create the catkin workspace:

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin init

3: dVRK install

On Ubuntu 20.04 you will need the following packages:

sudo apt install libxml2-dev libraw1394-dev libncurses5-dev qtcreator swig sox espeak cmake-curses-gui cmake-qt-gui git subversion gfortran libcppunit-dev libqt5xmlpatterns5-dev python3-wstool python3-catkin-tools python3-osrf-pycommon ros-noetic-rviz

Download the script that makes it easy to source ROS versions (already downloaded on VMs). Source ROS 1:
```
cd 
source ros_setup.sh -v 1
```

Download and install the dVRK (da Vinci Reserach Kit):

cd ~/catkin_ws                     # go in the workspace
wstool init src                    # we're going to use wstool to pull all the code from github
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release # all code should be compiled in release mode
cd src                             # go in source directory to pull code
wstool merge https://raw.githubusercontent.com/jhu-dvrk/dvrk-ros/master/dvrk_ros.rosinstall # or replace master by devel
wstool up                          # now wstool knows which repositories to pull, let's get the code
cd ~/catkin_ws
catkin build --summary             # ... and finally compile everything

Danger

Never use catkin build and catkin_make in the same workspace!

Start the RViz simulation of PSM1 (Patient Side Manipulator). Do not forget to HOME on the dVRK console. Start the ROS1-ROS2 Bridge. Study the simulator operation from ROS 2 using the learned prancs (ros2 topic list, ros2 topic echo ros2 run rqt_gui rqt_gui, etc.).

source ros_setup.sh -v 2
ros2 topic list
ros2 topic echo /PSM1/measured_cp
ros2 run rqt_gui rqt_gui

source ros_setup.sh -v 1
roslaunch dvrk_robot dvrk_arm_rviz.launch arm:=PSM1 config:=/home/$(whoami)/catkin_ws/src/cisst-saw/sawIntuitiveResearchKit/share/console/console-PSM1_KIN_SIMULATED.json

source ros_setup.sh -v b
ros2 run ros1_bridge dynamic_bridge --bridge-all-topics

source ros_setup.sh -v 2
ros2 run rqt_gui rqt_gui

4: PSM subscriber

Create a new python source file named psm_grasp.py in ~/ros2_ws/src/ros2_course/ros2_course. Specify the new entry point in setup.py in the usual way. ---
Subscribe to topics that publish the TCP (Tool Center Point) position of the PSM and the angle of the jaws of the tweezers.
```
/PSM1/measured_cp
/PSM1/jaw/measured_js
```

Build and run the node:

source ros_setup.sh -v 2
cd ~/ros2_ws
colcon build --symlink-install
ros2 run ros2_course psm_grasp

5. Moving PSM TCP along a linear trajectory

The PSM expects the desired TCP position and the angle closed by the jaws of the clamp in the topics below. Create publishers in psm_grasp.py for these topics.
```
/PSM1/servo_cp
/PSM1/jaw/servo_jp
```
Write a function that moves the TCP along a linear trajectory to the desired position. Send the pin to position (0.0, 0.05, -0.12), leave the orientation unchanged. The sampling time should be 0.01s. Using Matplotlib plot the planned trajectory x, y and z components of the projected trajectory as a function of time.
```
def move_tcp_to(self, target, v, dt):
```

Write a function to open and close the gripper, also using a linear trajectory.
```
def move_jaw_to(self, target, omega, dt):
```

6. Dummy marker

Create a new python source file named dummy_marker.py. Specify the entry point in setup.py in the usual way. Implement a python program that publishes a marker with position (-0.05, 0.08, -0.14) in topic dummy_target_marker. The value of the frame_id add tag should be PSM1_psm_base_link. Copy the following code into the file dummy_marker.py:

import rclpy
from rclpy.node import Node
from visualization_msgs.msg import Marker

class DummyMarker(Node):
    def __init__(self, position):
        super().__init__('minimal_publisher')
        self.position = position
        self.publisher_ = self.create_publisher(Marker, 'dummy_target_marker', 10)
        timer_period = 0.1  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0
        i = 0

    def timer_callback(self):
        marker = Marker()
        marker.header.frame_id = 'PSM1_psm_base_link'
        marker.header.stamp = self.get_clock().now().to_msg()
        marker.ns = "dvrk_viz"
        marker.id = self.i
        marker.type = Marker.SPHERE
        marker.action = Marker.MODIFY
        marker.pose.position.x = self.position[0]
        marker.pose.position.y = self.position[1]
        marker.pose.position.z = self.position[2]
        marker.pose.orientation.x = 0.0
        marker.pose.orientation.y = 0.0
        marker.pose.orientation.z = 0.0
        marker.pose.orientation.w = 1.0
        marker.scale.x = 0.008
        marker.scale.y = 0.008
        marker.scale.z = 0.008
        marker.color.a = 1.0 # Don't forget to set the alpha!
        marker.color.r = 0.0
        marker.color.g = 1.0
        marker.color.b = 0.0;

        self.publisher_.publish(marker)
        self.i += 1

def main(args=None):
    rclpy.init(args=args)
    marker_publisher = DummyMarker([-0.05, 0.08, -0.12])
    rclpy.spin(marker_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    marker_publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Run the node and display the marker in RViz.

7. Grasping the marker

Subscribe to the topic sending the marker position in psm_grasp.py.
Modify psm_grasp.py to use the tweezers to grasp the generated marker.

!!! note The simulator used has a tendency for certain values to get "stuck", so it is a good idea to reset the lever at the beginning of the program using the following lines:

#Reset the arm
psm.move_tcp_to([0.0, 0.0, -0.12], 0.01, 0.01)
psm.move_jaw_to(0.0, 0.1, 0.01)