#   conda activate coppeliasim
#   env fix, in terminal:  fix_robotics_env.sh

from  rm75_kinematics import rm75_kinematics

from math import pi
import numpy as np

#   pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
#   load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
tools_in_ee = {
    'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
    'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
    'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
    'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
}

# joint limit
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
lb = -ub

tool_name = "scissor"

def main():
    """Demonstrate pure position control"""

    # Create controller

    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)

    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)

    self_collision_sts = robot_kine.get_self_collision(q)

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

    print(f'self_collision_sts: {self_collision_sts}')





if __name__ == "__main__":
    main()