Practical use case: Implementing a custom motion profile

Recently I got into an interesting dilemma. The MOVE_JOINT_TO_VALUE FB is meant to be the way to move a joint based on the standard trapezoid motion profile. It does so pretty well but there is a catch there. The script only triggers the action. The motion itself is executed by a dedicated engine inside Process Simulate. The SCL script does not get any feedback for the progress and the timing of this engine. The end result is that users cannot tell when the motion will start or end. Usually they have to sacrifice a cycle or two just to have it running. It works at its own pace and eventually it moves the joint but timing is always unclear.

In this post I will try to present another approach to motion profiles using subscripts. They execute synchronously within the main script which means that they can provide feedback. I could find a nice pseudo code for the trapezoid motion profile online (https://www.ctrlaltftc.com/advanced/motion-profiling) and this is what I'll be using.

Having this pseudo code is exactly what I was looking for since writing this logic myself would have been really difficult. As you can see this is just one function that takes the velocity, the acceleration, the distance and the time on input and returns the position at this time. In SCL terms it looks like this:

FUNCTION "TRAPEZOID_MOP" : LREAL
VAR_INPUT
	ACC:LREAL;				// acceleration (mm/s, rad/s)
	VEL:LREAL; 				// velocity	 (mm/s, rad/s)
	DIST:LREAL;				// distance  (mm, rad)
	ELAPSED_TIME:LREAL;		// elapsed time (sec)
END_VAR
VAR_TEMP
	ACC_DT:LREAL;
	DEC_DT:LREAL;
	CRUISE_DT:LREAL;
	CRUISE_CUR_DT:LREAL;
	ENTIRE_DT:LREAL;
	HALF_DIST:LREAL;
	ACC_DIST:LREAL;
	CRUISE_DIST:LREAL;
	DEC_TIME:LREAL;
END_VAR
BEGIN
	// Calculate the time it takes to accelerate to max velocity
	#ACC_DT := #VEL / #ACC;	
	// If we can't accelerate to max velocity in the given distance, we'll accelerate as much as possible
	#HALF_DIST := #DIST / 2;
	#ACC_DIST := 0.5 * #ACC * #ACC_DT ** 2;
	IF (#ACC_DIST > #HALF_DIST) THEN
		#ACC_DT := SQRT(#HALF_DIST / (0.5 * #ACC));
	END_IF;
	#ACC_DIST := 0.5 * #ACC * #ACC_DT ** 2;
	// recalculate max velocity based on the time we have to accelerate and decelerate
	#VEL := #ACC * #ACC_DT;
	// we decelerate at the same rate as we accelerate
    #DEC_DT := #ACC_DT;
	// calculate the time that we're at max velocity
	#CRUISE_DIST := #DIST - 2 * #ACC_DIST;
	#CRUISE_DT := #CRUISE_DIST / #VEL;
    #DEC_TIME := #ACC_DT + #CRUISE_DT;
	// check if we're still in the motion profile
	#ENTIRE_DT := #ACC_DT + #CRUISE_DT + #DEC_DT;
	IF (#ELAPSED_TIME > #ENTIRE_DT) THEN
	    #TRAPEZOID_MOP := #DIST;
		RETURN;
	END_IF;
	// if we're accelerating
	IF (#ELAPSED_TIME  < #ACC_DT) THEN
		// use the kinematic equation for acceleration
	#TRAPEZOID_MOP:= 0.5 * #ACC * #ELAPSED_TIME ** 2;\
		RETURN;
	// if we're cruising
	ELSIF (#ELAPSED_TIME < #DEC_TIME) THEN
		#ACC_DIST := 0.5 * #ACC * #ACC_DT ** 2;
		#CRUISE_CUR_DT := #ELAPSED_TIME - #ACC_DT;
		// use the kinematic equation for constant velocity
		#TRAPEZOID_MOP := #ACC_DIST + #VEL * #CRUISE_CUR_DT;
		RETURN; 
	// if we're decelerating
	ELSE
		#ACC_DIST := 0.5 * #ACC * #ACC_DT ** 2;
		#CRUISE_DIST := #VEL * #CRUISE_DT;
		#DEC_TIME := #ELAPSED_TIME - #DEC_TIME;
		// use the kinematic equations to calculate the instantaneous desired position
		#TRAPEZOID_MOP := #ACC_DIST + #CRUISE_DIST + #VEL * #DEC_TIME - 0.5 * #ACC * #DEC_TIME ** 2;
		RETURN;
	END_IF;
END_FUNCTION

Save this to a UTF-8 encoded text file (.scl) and import it into PS by running the "Import SCL block" command. Older versions do not have this command so you need to put the file in the SCL Vault (EmPower\Scripting\SCL\Vault) yourself.

Open SCL editor and use this code on your resource:

FUNCTION_BLOCK "MAIN"
VERSION:1.0
VAR_OUTPUT
	pos : LREAL;
END_VAR
VAR
	t : LREAL;
END_VAR
BEGIN
    #t := SIM_TIME(); // take current time in ms
    #t := #t / 1000;  // ms to sec
    #pos := "TRAPEZOID_MOP"(VEL:=500, ACC:=200, DIST:=5000, ELAPSED_TIME:=#t); // calculate the position at this time
    JUMP_JOINT(NAME:='j1', VALUE:=#pos); // jump the joint
END_FUNCTION_BLOCK

Run the simulation and observe the joint value/speed in the Robot Viewer:

Tadaaa - the motion profile works!

You can repeat the process by resetting the elapsed time to zero.

If I have to mimic the MOVE_JOINT_TO_VALUE behavior using this script I would have to calculate the distance based on the current/target value of the joint and also manipulate the elapsed time for the motion profile. The negative/positive direction of movement should be taken into account. In other words the main script will become more complex. But once done it may become one more building block for us. Isn't this lovely. We can easily build our own primitives. What a marvel!

Unlike MOVE_JOINT_TO_VALUE now you know where the joint is at any given time! Isn't this great? Now you can build logic on top of this! Also provided you know the math, you can model any motion profile that you need. The only possible downside is performance. As you might expect the script won't be as fast as the built-in engine. The flexibility you get costs performance unfortunately.

See you in the next one!

Update 1: The TRAPEZOID_MOP function has a bug in the case where the input distance is set to zero. In this case it causes a division by zero error. To solve this simply add a check before making the calculations:

IF #DIST <> 0 THEN
	// ...
ELSE
	#TRAPEZOID_MOP := 0;
END_IF;