Wrong trajectory by using force mode

Hello,
I am having a problem using force mode. I have to draw on a plan with a marker.
I pilot the robot in force mode so as not to damage the marker and keep contact with the surface.
I calculate the position of different points. When I run the program the robot does not go to the calculated points.
The trajectories are wrong but reproducible. If I run the program without force mode everything is ok, the TCP goes on the calculate position.

Here is a part of my program:

freedrive_mode()

sleep (5.0)
end_freedrive_mode()

i_Reloc≔get_actual_tcp_pose()

p_contact_piece≔pose_trans(i_Reloc,p[0,0,0,0,0,0])
p_point1≔pose_trans(i_Reloc,p[0.105,0.14,0,0,0,0])
p_point3≔pose_trans(i_Reloc,p[0.105,0.49,0,0,0,0])
p_point22≔pose_trans(i_Reloc,p[-0.87,0.14,0,0,0,0])
p_point24≔pose_trans(i_Reloc,p[-0.87,0.49,0,0,0,0])

zero_ftsensor()
force_mode(tool_pose(), [0, 0, 1, 0, 0, 0], [0.0, 0.0, 2, 0.0, 0.0, 0.0], 2, [0.1, 0.1, 0.15, 0.17, 0.17, 0.17])

movel(p_contact_piece, a=1.2, v=0.25)
movel(p_point1, a=1.2, v=0.25)
movel(p_point22, a=1.2, v=0.25)
movel(p_point24, a=1.2, v=0.25)
movel(p_point3, a=1.2, v=0.25)
movel(p_point1, a=1.2, v=0.25)

end_force_mode()

In this picture you can see my test trajectory.

Please can you help me or tell me where I made a mistake.

Hi @jules.levesque,

I would expect your marker not to be completely perpendicular to the board. In that case your tool-z axis force will contribute to a translation on the plane of the board.