Driving joints

Kinematics is often referred to as the "geometry of motion". It relies on initial conditions (known values) of position velocity and/or acceleration to determine the same for the unknown parts of the system. External forces that cause the movement are ignored.

In Process Simulate the "Kinematics Editor" enables you to define links, joints and the related motion parameters. Joint values may be independent (driving) or may depend on the value of other joints. For each joint, limits (min/max values, max. speed and max. acceleration) can be defined.

In general there are two types of joints:

1. Revolute joints - enabling rotary motion between two links. base unit = radian

2. Prismatic joint - enabling linear motion between two links. base unit = millimeter

So much for the theory. Let's see how joints can be manipulated from within the SCL script. I emphasize on the script since it offers more flexibility when designing your logic (compared to LBs). e.g. the logic is expressed more easily and more naturally.

As you might imagine we have a set of functions/function blocks that can work on joints and poses. They all rely on the underlying resource being a kinematic device that has the joints/poses pre-defined. In the script we refer to the joints/poses by name (as defined in the kinematics/pose editor)

Functions:

1. JDS([in]NAME:STRING):LREAL - returns the current value of the joint. works on driving joints only. available since v15.1.1.

2. JUMP_JOINT([in]NAME:STRING, [in]VALUE:LREAL):VOID - sets the value of the joint. works on driving joints only. available since v15.1.1.

3. GET_JOINT_TYPE([in]NAME:STRING):INT - returns the type (1=revolute/2=prismatic) of the joint. works on driving/dependent joints. available since v2408.

4. GET_JOINT_LIMITS([in]NAME:STRING, [out]LOW:LREAL, [out]HIGH:LREAL):BOOL- returns the limits of the joint. works on driving/dependent joints. available since v2408.

Example:

FUNCTION_BLOCK "MAIN"
VERSION:1.0
VAR_INPUT
	h : LREAL;
END_VAR
VAR_TEMP
	low : LREAL;
	high : LREAL;
END_VAR
BEGIN

if GET_JOINT_TYPE('j1') = 2 (*prismatic*) then
	if GET_JOINT_LIMITS(NAME:='j1', LOW=>#low, HIGH=>#high) then
		if (#h >= #low and #h <= #high) then
			JUMP_JOINT(NAME:='j1', VALUE:=#h); // move the joint
		end_if;
	end_if;
end_if;

END_FUNCTION_BLOCK

Function blocks:

1. MOVE_TO_POSE - standard motion profile for poses. moves the device to a target pose. you can provide the speed/time for the motion. available since v15.1.1.

2. MOVE_JOINT_TO_VALUE - standard motion profile for joints. moves the joint to a target value. you can provide speed/acceleration/deceleration. available since v2301.

Example:

FUNCTION_BLOCK "MAIN"
VERSION:1.0
VAR_INPUT
	speed : LREAL;
	acc : LREAL;
	dec : LREAL;
	value : LREAL;
END_VAR
VAR
	mjv : MOVE_JOINT_TO_VALUE;
END_VAR
BEGIN

#mjv(NAME:='j1', 
  	SPEED:=#speed, 
   	ACCELERATION:=#acc, 
   	DECELERATION:=#dec, 
   	VALUE:=#value);
	
END_FUNCTION_BLOCK

The standard motion profile tries to reach the target joint value while moving at the target speed. Acceleration is applied while the target speed is not reached. Deceleration is applied before reaching the target joint value. Of course the difference in current/target joint values (distance) should be large enough to allow for the target speed to be reached. Otherwise the picture may look quite differently.

Believe it or not comparing joint values is a problem since we deal with floating point numbers. When you compare you need to use some tolerance around the target value. Otherwise you may get strange results. The tolerance depends on the joint's type.

Example:

if #jointType = 2 then
	#eps := 0.001; 	// tolerance for prismatic joints [mm]
else
	#eps := 0.00087; 	// tolerance for revolute joints [rad]
end_if;

if ABS(#targetValue - #jointValue) < #eps then
	; // target value reached
end_if;

This is a repetitive code that is suitable for a function.

I'll leave that exercise to you.