Add and update the scripts for the new instrument, kinematic data record & replay

[JackHaoyingZhou]

The pull request contributes to the following points:

1. make the scale of everything to be 1:1 (change linear scaling factor from 10 to 1).
2. add kinematic data recording and replaying scripts, correspond the replay scaling factor with the unit conversion information.
3. add an additional argument called "tool_id" to address kinematics and teleoperation scripts for different surgical instrument

Need to do:

1. add new models (include new suturing needle, 3d med phantom, new surgical instrument and etc)
2. I only provide the closed-form FK, but I also have scripts for FK using screw theory, can add to the repository if required
3. the scripts suppose to work with new models, will test after adding the new models.

[JackHaoyingZhou]

Merge FK&IK method into the psmKinematic solver class; create a class variable for the teleoperation scaling factor; in the later future, create a json file to load all tools; move the grasp needle into utility or examples

[JackHaoyingZhou]

Revision done, clean code, ready to merge

[JackHaoyingZhou]

Wait for asset so that we can do the final merge

[jabarragann]

Hi Adnan/Jack,

I went over Jack's PR and had some small comments and possible changes.

• Currently we copy-pasted the JSON files from the dVRK into the SRC repository, but not all the values from these JSON files are used. Only the DH parameters are read from these files. Should we try to simplify them? That is only keeping the values that are used inside the SRC code base.

• Add a readme on the config folder indicating that 4200006 is an SI large needle driver and that 4000006 is a classic large needle driver.

• Maybe change the names of kinematic files to include the AMBF_large_needle_driver.json. This way we make sure people know these are only for the simulation. Also, specify in the readme which DH parameters are different from the ones in the DVRK repository.

Future developments

• Adding to ROS parameters the tool name that is loaded in the simulation so that the FK script can automatically load the correct JSON files. Similar to the way the dVRK loads its kinematic parameters.

[jabarragann]

@adnanmunawar and @JackHaoyingZhou a small change before submitting the PR:

1. the teleoperation scripts names are mtm_psm_pair_teleop_400006.sh and mtm_psm_pair_teleop_420006.sh, but the script to run the environment are run_env_simple_instruments.sh and run_env_real_instruments.sh. I would make sure these names consistent.

Probably users won't know that simple instruments are based on the 400006 instrument.

2. Revise all scripts that included FK and IK functionalities. There might be some broken scripts.