Implement inverse kinematic for the tiago arm

Now that we know how to plan a trajectory in the joint space, we need to be able to plan a trajectory to reach a specific cartesian point in the 3d space.

You task is to transform from a 3D position to a joint configuration such that the gripper_grasping_frame is at the specified position. The frame is shown in the rviz screenshot below. The 3D point can be represented in the arm_1_link frame, in this way you do not have to take into account the rest of the robot's kinematic structure except the remainig 6 links.

You can get the xacro of the arm, end_effector and gripper description from the xacro files in src/week8/config/.