Model predicting robot offsets not learning

username: sabeaussan
cooked: <p>Hi,</p>
<p>I am currently trying to predict a serial robot’s offsets by using a neural network on Pytorch. The method is the following :</p>
<ul>
<li>Sampling random a vector of joint_angles.</li>
<li>Feeding neural network with it in order to predict corresponding offsets.</li>
<li>Computing FK(joint_angles+offsets) to get estimated_effector_position corresponding to the estimated joints states.</li>
<li>Moving the robot with joint_angles and measure real_effector_position which contains informations about the real offsets</li>
<li>Compute MSE(real_effector_position,estimated_effector_position) and then backprop error.<br>
The network should output offsets that minimize the error with training.</li>
</ul>
<p>Formally, I’m trying to solve the following optimization problem :<br>
arg min(offsets) ||FK(joint_angles+offsets) - real_effector_position||</p>
<p>I’m using pytroch to implement this but the network doesn’t learn and the loss stay the same. Here is my code</p>
<pre><code>robot = BraccioRobot()
loss = 0
model = OffsetMLP(3,64)
opti = optim.Adam(net.parameters(), lr=3e-4)
for i in range(10000): 
    desired_joints_state = torch.FloatTensor(np.random.randint(0,180,(10,3))) # Get joint_angles
    offset = model(desired_joints_state/180)  * 15                            # Predict offset
    angles = desired_joints_state + offset                                    # Effective angles
    x,y = robot.forward_kinematic(((angles-90)*np.pi)/180)                    # Compute FK with offsets
    x_t,y_t = robot.offset_forward_kinematic(((desired_joints_state-90)*np.pi)/180) # Real joints_states
    loss = F.mse_loss(x,x_t) + F.mse_loss(y,y_t)                              # Compute loss
    print(loss)
    opti.zero_grad()
    loss.backward()
    opti.step()
</code></pre>
<p>the robot class :</p>
<pre><code>class BraccioRobot():
    def __init__(self,) :
        self.proportions = np.array([0.17,0.29,0.285,0.25])
        self.joints_offsets = np.array([10,9,8]) * (np.pi/180.0)
    
    def forward_kinematic(self,joint_states):
        # Compute standard FK
        parent_angle = 0
        x = torch.zeros(joint_states.shape[0])
        y = torch.zeros(joint_states.shape[0])
        for i in range(joint_states.shape[1]):
            x += self.proportions[i+1] * torch.sin(joint_states[:,i] + parent_angle)
            y += self.proportions[i+1] * torch.cos(joint_states[:,i] + parent_angle)
            parent_angle += joint_states[:,i]
        y += self.proportions[0]
        return x,y # Effector position 

    def offset_forward_kinematic(self,joint_states):
        # Compute FK with added offsets
        parent_angle = 0
        x = torch.zeros(joint_states.shape[0])
        y = torch.zeros(joint_states.shape[0])
        for i in range(joint_states.shape[1]):
            x += self.proportions[i+1] * torch.sin(joint_states[:,i] + parent_angle + self.joints_offsets[i])
            y += self.proportions[i+1] * torch.cos(joint_states[:,i] + parent_angle + self.joints_offsets[i])
            parent_angle += joint_states[:,i]
        y += self.proportions[0]
        return x,y # Effector position
</code></pre>
<p>and the model :</p>
<pre><code>class OffsetMLP(nn.Module):

    def __init__(self,n_joints,n_hidden):
        super(OffsetMLP,self).__init__()
        self.fc1 = nn.Linear(n_joints,n_hidden)
        self.fc2 = nn.Linear(n_hidden,n_joints)
        self.outputAct = nn.Tanh()

    def forward(self,desired_joints_state):
        offset = self.fc1(desired_joints_state)
        offset = F.relu(offset)
        offset = self.fc2(offset)
        offset = self.outputAct(offset)
        return offset
</code></pre>
<p>the offset_forward_kinematic function is used to emulate real robot movement. It just add the offsets that the network must learn. In this case it is 3 simple constant. I’m guessing the error is somewhere in the forward_kinematic function with the gradient maybe not propagating as it should but I can’t seem to find a solution. Thanks in advance for any fix.</p>