Hello I am currently running path following admittance controller on the UR5. I am trying to find a way to do the following. Given the initial position/orientation of the TCP and the initial linear/angular velocity of the TCP, i wish to estimate how long it takes until the robot reaches some target endpoint. The way that I do this now is by simply running a simulation in python that mimics the control loop for the robot and seeing how many time steps it takes until I am sufficiently close to the desired end point. However my simulation is fairly inacccuracte and I suspect this has to do with the way that I model the speedL command.
On the official website it gives the following description:
bool speedL(const std::vector &xd, double acceleration = 0.25, double time = 0.0)
Tool speed - Accelerate linearly in Cartesian space and continue with constant tool speed.
The time t is optional;
Parameters
xd: tool speed [m/s] (spatial vector)
acceleration: tool position acceleration [m/s^2]
time: time [s] before the function returns (optional)
And as I understand it this seems to accelerate from the current velocity to the target velocity adn then remain at such target velocity. However I am unsure of how it interacts with the angular velocity / acceleration of the TCP.
I have written the following code to attempt to simulate the effects of the UR5 command:
def sim_speedL(pose: np.ndarray, cur_v: np.ndarray, target_v: np.ndarray, accel: float,
sim_dt: float) → np.ndarray:
“”"
Simulates the speedL command on the UR5
Accelerates linearly from the current to target velocity adn then remains at target velocity until
the end of the time step(whose length is given by sim_dt)
Returns the updated pose of the (simulated) UR5
“”"
diff_vec = target_v - cur_v
a_time = np.linalg.norm(diff_vec) / accel
a_vec = diff_vec / np.linalg.norm(diff_vec)
del_p1 = cur_v * a_time + a_vec * accel / 2 * a_time * 2
del_p2 = target_v * (sim_dt - a_time)
new_pose = pose + del_p1 + del_p2
return new_pose
However this seems to be the source of error. I suspect i have misinterpreted how the speedL command works but I have struggled ot find the source code or more details online. If anyone could help me find my mistake or point me to good simulaiton library it would be incredibly helpful!!
(As a short endnote: the reason i am not using any preexisting libraries is that I wish for my simulation to be fast. Ideally it would be able to simulate 4-5 seconds of motion in abt 0.2 seconds of computation, as the total runtime would optimally be contiuosy updated to take into account the change in position