Modeling

We first need to define the joint coordinate q we will use eventually. This q is the joint motion we collect from the robot and corresponding to the joint torque \tau we use to control the robot.