Controlling dVRK Manipulators
This is an example of how to begin using the AMBF Simulator. Let's start by going to the launch file in your local cloned repository. This launch file is located in ambf/ambf_models/descriptions/launch.
# This is the base file for Coordination Application
world config: ./world/world.yaml
color config: ./color/colors.yaml
input devices config: ./input_devices/input_devices.yaml
multibody configs:
- "./multi-bodies/robots/blender-toy-car.yaml" #0
- "./multi-bodies/robots/blender-toy-car2.yaml" #1
- "./multi-bodies/robots/blender-neuro-robot.yaml" #2
- "./multi-bodies/robots/blender-raven2.yaml" #3
- "./multi-bodies/robots/blender-mtm.yaml" #4
- "./multi-bodies/robots/blender-psm.yaml" #5
- "./multi-bodies/robots/blender-ecm.yaml" #6
- "./multi-bodies/robots/blender-suj.yaml" #7
- "./multi-bodies/robots/blender-kuka.yaml" #8
- "./multi-bodies/robots/blender-pr2.yaml" #9
- "./multi-bodies/puzzles/parallel_structure.yaml" #10
- "./multi-bodies/puzzles/puzzle1.yaml" #11
- "./multi-bodies/puzzles/puzzle2.yaml" #12We can see that the dVRK Manipulators are defined at Index 4 (MTM), 5 (PSM) and 6 (ECM).
Now let's launch the simulator as follows:
cd ~/ambf/bin/<os>
./ambf_simulator -l 4,5,6
Do you see the dVRK ECM, PSM, and MTM as shown below? If yes, great, otherwise close the simulator and double check to see that the indexes match the dVRK manipulators in your ambf/ambf_models/descriptions/launch file.
As a demo, some premade RQT files have been placed in ambf/ambf_utilities/rqt_perspectives to get you up and running.
In a terminal, launch rqt_gui using:
rosrun rqt_gui rqt_gui
Now in the top menu of rqt_gui, navigate to Perspectives -> Import
From the file browser, head to ambf/ambf_utilities/rqt_perspectives and select the ecm-psm-mtm-rqt-gui.perspective file.
Note that AMBF could be installed in a non-default path in your system, so make sure to go to the correct path.
Great. You should see some topics already visible for selection in the rqt_gui. Select the checkbox for any topics to see the effect on the corresponding dVRK manipulator. Uncheck the checkbox to reset the commands. You can expand the topics in the RQT Gui to see what kind of command are we issuing to the manipulators.
Now let's use the provided python client for finer control of the manipulators. you can use the following python code snippet:
# Import the Client from ambf_client package
from ambf_client import Client
import time
# Create a instance of the client
_client = Client()
# Connect the client which in turn creates callable objects from ROS topics
# and initiates a shared pool of threads for bi-directional communication
_client.connect()
print('\n\n----')
raw_input("We can see what objects the client has found. Press Enter to continue...")
# You can print the names of objects found. We should see all the links found
print(_client.get_obj_names())
# Lets get a handle to PSM and ECM, as we can see in the printed
# object names, 'ecm/baselink' and 'psm/baselink' should exist
ecm_handle = _client.get_obj_handle('ecm/baselink')
psm_handle = _client.get_obj_handle('psm/baselink')
# Similarly we can get a handle to any link lower in the hierarchy rather
# than the root link. Let's get a handle to MTMs wrist platform link
mtm_wrist_handle = _client.get_obj_handle('mtm/WristYaw')
# Let's sleep for a very brief moment to give the internal callbacks
# to sync up new data from the running simulator
time.sleep(0.2)
print('\n\n----')
raw_input("Let's Get Some Pose Info. Press Enter to continue...")
# Not we can print the pos and rotation of object in the World Frame
print('ECM Base Pos:')
print(ecm_handle.get_pos())
print(' ')
print('PSM Base Rotation as Quaternion:')
print(psm_handle.get_rot())
print(' ')
print('PSM Joint 0 (baselink-yawlink joint) Position by passing Joint Index as a parameter:')
print(psm_handle.get_joint_pos(0))
print(' ')
print('PSM baselink-yawlink joint (Joint 0) Position by passing Joint Name as a parameter :')
print(psm_handle.get_joint_pos('baselink-yawlink'))
print(' ')
print('MTM Wrist Fixed Rotation:')
print(mtm_wrist_handle.get_rpy())
print('\n\n----')
raw_input("Let's get Joints and Children Info. Press Enter to continue...")
# We can get the number of children and joints connected to each object as
ecm_num_joints = ecm_handle.get_num_joints() # Get the number of joints of this object
psm_children_names = psm_handle.get_children_names() # Get a list of children names belonging to this obj
print('Number of Joints in ECM:')
print(ecm_num_joints)
print(' ')
print('Name of PSM\'s children:')
print(psm_children_names)
print('\n\n----')
raw_input("Control ECMs joint positions. Press Enter to continue...")
# Now let's control some joints
# The 1st joint, which is usually the ECM Yaw
# Joint Index values may vary and can be found out by checking the ecm_handle.get_joint_names()
ecm_handle.set_joint_pos(0, 0)
# The 2nd joint, which is usually the ECM Pitch
ecm_handle.set_joint_pos(1, -0.2)
# The 3rd Kinematic or 7th Actual Joint, which is usually the Prismatic Insertion Joint
ecm_handle.set_joint_pos(6, 0.1)
print('\n\n----')
raw_input("Mixed Pos and Effort control of PSM\'s joints. Press Enter to continue...")
# For the PSM let's control some in position and some in effort mode
# The 1st joint, which is usually the PSM Yaw
# Joint Index values may vary and can be found out by checking the psm_handle.get_joint_names()
psm_handle.set_joint_effort(0, 0.5)
# The 3rd Kinematic joint, which is usually the PSM Insertion Joint
psm_handle.set_joint_pos(3, -0.2)
print('\n\n----')
raw_input("Set force on MTM's Wrist Yaw link for 5 secs. Press Enter to continue...")
# Let's directly control the forces and torques on the mtmWristYaw Link
# Notice that these are in the world frame. Another important thing to notice
# is that unlike position control, forces control requires a continuous update
# to meet a watchdog timing condition otherwise the forces are reset Null. This
# is purely for safety reasons to prevent unchecked forces in case of malfunctioning
# python client code
for i in range(0, 500):
mtm_wrist_handle.set_force(0, 0, 10) # Set 10 N in the World Z axis
time.sleep(0.01) # Run the loop for 10 seconds
print('\n\n----')
raw_input("Set wrench on MTM's Wrist Yaw link for 5 secs. Press Enter to continue...")
# Similarly we can individually apply the torque
for i in range(0, 500):
mtm_wrist_handle.set_torque(0, 3, 0) # Set 10 Nm in the World Y axis
time.sleep(0.01) # Run the loop for 10 seconds
print('\n\n----')
raw_input("Let's clean up. Press Enter to continue...")
# Lastly to cleanup
_client.clean_up()