### This repo is for inverse kinematics and verification

In this branch, the **integrated** inverse kinematics method is packed into one python class **rm75_kinematics** in ``rm75_kinematics.py``. 

The user can call it as in `test1.py`, `test2.py`.

How to use

```aiignore
from  rm75_kinematics import rm75_kinematics
robot_kine = rm75_kinematics(urdf_path='./urdf_rm75/RM75-SCI.urdf',
                             mesh_dir='./urdf_rm75',
                             tcps=["scissor_tcp", "camera_tcp"],
                             tools_in_ee=tools_in_ee,min_j=lb, max_j=ub)
```

Parameter definition:

- urdf_path: the robot description file, ending with `.urdf`.
- mesh_dir: the robot parts, ending with `.stl`.
- tcps: the tool central points defined in urdf file, if no, ignore it.
- tools_in_ee: the installation of different tools attached to the end-effector of the arm (jont7+link7).


### Current functions ###

1. `get_ik_result`
```aiignore
ret_ik, q = robot_kine.get_ik_result(target_position=[0.2, -0.2 , 0.5 ], 
                                    target_rpy=[0.2022060487764064, -0.0097962261845583, -0.6518417572686532],
                                    initial_guess=[0.1] * 7, tool=tool_name)
```

2. `get_fk_result`
```aiignore
p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)
```

3. `get_self_collision`
```aiignore
self_collision_sts = robot_kine.get_self_collision(q)
```