﻿ RCFS packages = ['numpy']

# Exercise 4.aForward kinematics

Forward kinematics is the problem of finding the pose of the robot given a configuration (joint angles). You can read about forward kinematics in Section 4 of the RCFS documentation.

The goal of this exercise is to implement a forward kinematics function for the manipulator shown on the right. The pink circle will be used to represent the end-effector of the robot.

If the forward kinematics function is correctly implemented, this pink circle should coincide with the end-effector of the robot. You can move the robot joints by hovering the mouse over each joint and using the mousewheel and/or the touchpad.

The function `fkin` represents the forward kinematics function taking the current joint angles `x` and problem parameters class `param` as inputs and outputs a 2 dimensional vector representing the 2D position of the end-effector. Currently, `f=np.zeros(2)` and if you run the code in the Question tab, you will see that the pink object appears at the base of the robot, at coordinates (0,0).

Note that the `param` class has attributes such as the number of joints (`param.nbVarX`) and the link lengths (`param.l`) that you can use to test your code.

• Change this function to implement forward kinematics.
• Verify your code by changing the number of joints and the link lengths.

param.nbVarX = 3 # State space dimension (x1,x2,x3) param.l = np.ones(param.nbVarX) * 800 / param.nbVarX # Robot links lengths x = -np.ones(param.nbVarX) * np.pi / param.nbVarX # Initial robot state x[0] = x[0] + np.pi # Forward kinematics for end-effector (in robot coordinate system) def fkin(x, param): f = np.zeros(2) return f
param.nbVarX = 3 # State space dimension (x1,x2,x3) param.l = np.ones(param.nbVarX) * 800 / param.nbVarX # Robot links lengths x = -np.ones(param.nbVarX) * np.pi / param.nbVarX # Initial robot state x[0] = x[0] + np.pi # Forward kinematics for end-effector (in robot coordinate system) def fkin(x, param): L = np.tril(np.ones([param.nbVarX, param.nbVarX])) f = np.array([param.l @ np.cos(L @ x), param.l @ np.sin(L @ x)]) return f