fix the IK and FK bug you can control the robot well
TODO: add the ROT on World Axiz