robots

Core robot class implementation.

This module defines a robot class that represents a robotic system with various configurations, transformations, and control mechanisms. It provides methods for controlling the robot’s movement, updating its state, handling the tool center point (TCP), managing sensors and grippers, as well as various kinematic and control strategies. The class includes support for joint and task space motion, force/torque sensor integration, and multiple motion control strategies, allowing for flexible and complex robot operations.

Key functionalities include: - Motion control in joint and task space (Cartesian, Object, Robot, World). - Force/torque sensor handling. - Gripper attachment and detachment. - Asynchronous and synchronous motion control. - Robot state and pose management. - Path planning with advanced kinematic computations. - Trajectory and null-space control.

class robotblockset.robots.MotionResultCodes(*values)[source]

Bases: IntEnum

Result codes returned by robot motion commands.

Vrednosti

MOTION_SUCCESSint: 0. Motion completed successfully.
MOTION_FAILUREint: 1. Motion failed.
MOTION_ABORTEDint: 2. Motion was aborted.
JOINT_LIMITSint: 3. A joint limit was reached.
CLOSE_TO_TARGETint: 4. The robot is already close to the requested target.
ACTIVE_THREADSint: 5. Active worker threads prevent execution of the command.
WRONG_STRATEGYint: 6. The selected motion strategy is not valid for the requested command.
NOT_FEASIBLEint: 7. The requested motion is not feasible.
NO_ROBOT_ATTACHEDint: 8. No robot is attached or available.
RTDE_ERRORint: 9. An RTDE communication error occurred.
NO_RESPONSEint: 10. No response was received from the robot or controller.
NOT_READYint: 11. The system is not ready to execute motion.

MOTION_SUCCESS = 0

MOTION_FAILURE = 1

MOTION_ABORTED = 2

JOINT_LIMITS = 3

CLOSE_TO_TARGET = 4

ACTIVE_THREADS = 5

WRONG_STRATEGY = 6

NOT_FEASIBLE = 7

NO_ROBOT_ATTACHED = 8

RTDE_ERROR = 9

NO_RESPONSE = 10

NOT_READY = 11

robotblockset.robots.MotionResultStr(code: MotionResultCodes | int) → str[source]

Convert motion result code to a descriptive human-readable string.

Parameters:: code (Union[MotionResultCodes, int]) – Result code to describe.
Returns:: Human-readable description of the result code.
Return type:: str

class robotblockset.robots.CommandModeCodes(*values)[source]

Bases: Enum

Major command mode codes used by robot motion commands.

Attributes:

ABORT (float) – -2. Abort motion immediately.
WAIT (float) – -1. Hold position and wait.
STOP (float) – 0. Stop motion and maintain the current position.
START (float) – 0.5. Start motion and enter the preparation phase.
JOINT (float) – 1. Joint-space motion control.
JOINT_PATH (float) – 1.1. Follow a predefined joint-space path.
JOINT_RBFPATH (float) – 1.2. Follow a joint-space path defined by a radial basis function.
JOINT_TRAJ (float) – 1.3. Follow a generated joint-space trajectory.
CARTESIAN (float) – 2. Cartesian motion control.
CARTESIAN_NONE (float) – 2.1. Cartesian motion without null-space optimization.
CARTESIAN_MANUPULABILITY (float) – 2.2. Cartesian motion with manipulability optimization.
CARTESIAN_JOINTLIMITS (float) – 2.3. Cartesian motion with joint-limit avoidance.
CARTESIAN_CONFIGURATION (float) – 2.4. Cartesian motion with configuration optimization.
CARTESIAN_POSE (float) – 2.5. Cartesian pose-based control.
CARTESIAN_TASKVELOCITY (float) – 2.6. Cartesian velocity control in task space.
CARTESIAN_JOINTVELOCITY (float) – 2.7. Cartesian motion with joint-space velocity control.
CARTESIAN_USER (float) – 2.9. Cartesian motion with a user-defined optimization rule.
PLANAR (float) – 3. Generic planar motion control.
PLANAR_POS (float) – 3.1. Planar position control.
PLANAR_ORI (float) – 3.2. Planar orientation control.
PLANAR_LOCATION (float) – 3.3. Planar motion to a specific location.
PLANAR_FORWARD (float) – 3.4. Planar forward motion.
PLANAR_TURN (float) – 3.5. Planar turning motion.
AUTONOMOUS (float) – 4. Autonomous path planning or execution.
JOGGING (float) – 5. Manual jogging or teach-mode movement.
INTERNAL_JOINT (float) – 11. Internal joint-space control.
INTERNAL_JOINT_CARTESIAN (float) – 11.1. Internal joint-space control for task-space motion.
INTERNAL_CARTESIAN (float) – 12. Internal Cartesian control.
INTERNAL_CARTESIAN_JOINT (float) – 12.1. Internal Cartesian control for joint-space motion.

ABORT = -2

WAIT = -1

STOP = 0

START = 0.5

JOINT = 1

JOINT_PATH = 1.1

JOINT_RBFPATH = 1.2

JOINT_TRAJ = 1.3

CARTESIAN = 2

CARTESIAN_NONE = 2.1

CARTESIAN_MANUPULABILITY = 2.2

CARTESIAN_JOINTLIMITS = 2.3

CARTESIAN_CONFIGURATION = 2.4

CARTESIAN_POSE = 2.5

CARTESIAN_TASKVELOCITY = 2.6

CARTESIAN_JOINTVELOCITY = 2.7

CARTESIAN_USER = 2.9

PLANAR = 3

PLANAR_POS = 3.1

PLANAR_ORI = 3.2

PLANAR_LOCATION = 3.3

PLANAR_FORWARD = 3.4

PLANAR_TURN = 3.5

AUTONOMOUS = 4

JOGGING = 5

INTERNAL_JOINT = 11

INTERNAL_JOINT_CARTESIAN = 11.1

INTERNAL_CARTESIAN = 12

INTERNAL_CARTESIAN_JOINT = 12.1

robotblockset.robots.CommandModeStr(mode: CommandModeCodes | float | int) → str[source]

Convert command mode code to a descriptive human-readable string.

Parameters:: mode (Union[CommandModeCodes, float, int]) – Command mode to describe.
Returns:: Human-readable description of the command mode.
Return type:: str

class robotblockset.robots.robot(**kwargs: Any)[source]

Bases: rbs_object

Represents a robot master class with various configurations, transformations, and control mechanisms.

Attributes:

Name (str) – Name of the robot.
tsamp (float) – Sampling rate for the robot.
TCP (np.ndarray) – Transformation matrix for the robot’s Tool Center Point (TCP).
TBase (np.ndarray) – Robot’s base pose transformation matrix (4, 4).
vBase (np.ndarray) – Robot’s base velocity twist (6,).
TObject (np.ndarray) – Transformation matrix for the object the robot manipulates.
TCPGripper (np.ndarray) – Transformation matrix for the robot’s gripper nominal TCP.
Tool (tool_params) – Parameters of the selected tool.
Load (load_params) – Load associated with the robot.
Gripper (Optional[Any]) – Gripper object attached to the robot, if any.
FTSensor (Optional[Any]) – Force/Torque sensor attached to the robot, if any.
FTSensorFrame (np.ndarray) – Transformation matrix of the F/T sensor frame relative to the end-effector.
FTFrame (np.ndarray) – Transformation matrix relative to flange in which forces and torques are expressed
Platform (Optional[Any]) – Platform object to which the robot is attached.
User (Optional[Any]) – User-defined data or object.
Tag (Optional[Any]) – Tag associated with the robot.
Motion_result_code (_motion_result_codes) – RBS Result codes reported by motion commands

Initializes the robot with default values and optional configurations.

Parameters:: **kwargs (Any) – Optional arguments for custom configuration or parameters.

__init__(**kwargs: Any) → None[source]

Initializes the robot with default values and optional configurations.

Parameters:: **kwargs (Any) – Optional arguments for custom configuration or parameters.

reset_threads() → None[source]

Resets the internal semaphore to its initial state.

This method reinitializes the semaphore with a value of 1, effectively allowing one thread to acquire it.

Return type:: None

jointvar(x: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Validates and returns the input x if it has the proper shape for joint positions.

Parameters:: x (ArrayLike) – Input array representing joint positions, which must have the shape (n, nj), where n is the number of samples and nj is the number of joints.
Returns:: The input array x if it has the correct shape.
Return type:: JointConfigurationType
Raises:: TypeError – If the input x does not have the proper shape.

Note

Assuming ‘nj’ represents the number of joints, defined elsewhere in the class

spatial(x: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...]) → ndarray[source]

Validates the shape of the input x and returns it in an appropriate format.

Parameters:

x (ArrayLike) –

The input array representing a spatial quantity, which can be one of the following shapes:

(7,) : pose (position and quaternion)
(4, 4) : transformation matrix
(3,) : position vector
(4,) : quaternion
(3, 3) : rotation matrix
(6,) : twist (linear and angular velocity)
(3, 4) : homogeneous matrix without the last row (assumed to be 3x4)

Returns:

The input x in the validated shape, possibly modified if the shape was (3, 4).

Return type:

np.ndarray

Raises:

TypeError – If the input x does not have a valid shape.

simtime() → float[source]

Returns the current simulation time based on the system’s performance counter.

The perf_counter function provides a high-resolution timer that is useful for measuring time intervals. It is system-dependent and returns the time as a floating-point value in seconds.

Returns:: The current simulation time in seconds since an arbitrary point (usually the start of the program).
Return type:: float

UseThreads(active: bool) → None[source]

Enables or disables the use of threads for robot control.

This method sets the _threads_active attribute, which controls whether the robot’s control system should use threads for asynchronous operations. When threads are enabled, the robot will perform certain operations in parallel to improve performance, especially during control updates or simulations.

Parameters:: active (bool) – A boolean value that indicates whether threads should be active. If True, threads are enabled. If False, threads are disabled.
Returns:: This method does not return any value.
Return type:: None

SetTsamp(tsamp: float) → None[source]

Set the sampling time (tsamp) for the robot control system.

This method updates the tsamp attribute, which is used to define the time interval between control updates or simulations. Additionally, it updates the Wait attribute in the _default object to reflect the new sampling time.

Parameters:: tsamp (float) – The new sampling time in seconds to set for the robot control system.
Returns:: This method does not return any value.
Return type:: None

ResetTime() → None[source]

Reset the robot’s time to the current simulation time.

This method updates the internal time attributes _t0 and _tt0 to the current simulation time and the current robot time, respectively. It also retrieves the robot’s state and updates any internal state information.

Returns:: This method does not return any value.
Return type:: None

isConnected() → bool[source]

Checks if the robot is connected.

Returns:: True if the robot is connected, False otherwise.
Return type:: bool

isReady() → bool[source]

Check if the robot is ready for operations.

This method checks the _connected attribute to determine if the robot is connected and operational.

Returns:: True if the robot is connected and ready for operations, otherwise False.
Return type:: bool

isActive() → bool[source]

Check if the robot is active.

This method always returns True, indicating that the robot is in an active state. It can be overridden in subclasses for more complex behavior.

Returns:: Always returns True, indicating the robot is active.
Return type:: bool

inMotion() → bool[source]

Check if the robot is in motion.

Returns:: True indicating the robot is excetuting motion command.
Return type:: bool

Check(silent: bool = False) → list[str][source]

Checks the status of the robot.

Parameters:: silent (bool, optional) – If True, suppress status messages while checking the robot state.
Returns:: A list containing the list of non-active status and status description.
Return type:: list[str]

HasError() → bool[source]

Checks is robot has errors

Returns:

——–
bool – True is robots has erros.

property Time: float

Get the elapsed wall time since the robot was initialized.

Returns:: Elapsed time in seconds.
Return type:: float

property t: float

Get the elapsed time since the robot was initiated.

Returns:: Time difference in seconds.
Return type:: float

property trob: float

Get the elapsed robot time since the robot was initiated.

Returns:: Time difference in seconds.
Return type:: float

property command: _command

Get the commanded state of the robot.

Returns:: A copy of the commanded state.
Return type:: _command

property actual: _actual

Get the actual state of the robot.

Returns:: A copy of the actual state.
Return type:: _actual

property q: ndarray

Get the current joint positions.

Returns:: Joint positions (nj,).
Return type:: np.ndarray

property qdot: ndarray

Get the current joint velocities.

Returns:: Joint velocities (nj,).
Return type:: np.ndarray

property trq: ndarray

Get the current joint torques.

Returns:: Joint torques (nj,).
Return type:: np.ndarray

property trqExt: ndarray

Get the current external joint torques.

Returns:: External joint torques (nj,).
Return type:: np.ndarray

property x: ndarray

Get the current end-effector pose.

Returns:: End-effector pose (7,) or (…, 7).
Return type:: np.ndarray

property p: ndarray

Get the current end-effector position.

Returns:: End-effector position (3,).
Return type:: np.ndarray

property Q: ndarray

Get the current end-effector quaternion.

Returns:: End-effector quaternion (4,).
Return type:: np.ndarray

property R: ndarray

Get the current end-effector rotation matrix.

Returns:: End-effector rotation matrix (3, 3).
Return type:: np.ndarray

property T: ndarray

Get the current end-effector transformation matrix.

Returns:: End-effector transformation matrix (4, 4).
Return type:: np.ndarray

property v: ndarray

Get the current end-effector velocity.

Returns:: End-effector velocity (6,).
Return type:: np.ndarray

property pdot: ndarray

Get the current end-effector linear velocity.

Returns:: End-effector linear velocity (3,).
Return type:: np.ndarray

property w: ndarray

Get the current end-effector angular velocity.

Returns:: End-effector angular velocity (3,).
Return type:: np.ndarray

property FT: ndarray

Get the current force/torque sensor data.

Returns:: Force/Torque sensor data (6,).
Return type:: np.ndarray

property F: ndarray

Get the current force sensor data.

Returns:: Force sensor data (3,).
Return type:: np.ndarray

property Trq: ndarray

Get the current torque sensor data.

Returns:: Torque sensor data (3,).
Return type:: np.ndarray

property q_ref: ndarray

Get the desired joint positions.

Returns:: Desired joint positions (nj,).
Return type:: np.ndarray

property qdot_ref: ndarray

Get the desired joint velocities.

Returns:: Desired joint velocities (nj,).
Return type:: np.ndarray

property x_ref: ndarray

Get the desired end-effector pose.

Returns:: Desired end-effector pose (7,).
Return type:: np.ndarray

property p_ref: ndarray

Get the desired end-effector position.

Returns:: Desired end-effector position (3,).
Return type:: np.ndarray

property Q_ref: ndarray

Get the desired end-effector quaternion.

Returns:: Desired end-effector quaternion (4,).
Return type:: np.ndarray

property R_ref: ndarray

Get the desired end-effector rotation matrix.

Returns:: Desired end-effector rotation matrix (3, 3).
Return type:: np.ndarray

property T_ref: ndarray

Get the desired end-effector transformation matrix.

Returns:: Desired end-effector transformation matrix (4, 4).
Return type:: np.ndarray

property v_ref: ndarray

Get the desired end-effector velocity.

Returns:: Desired end-effector velocity (6,).
Return type:: np.ndarray

property pdot_ref: ndarray

Get the desired end-effector linear velocity.

Returns:: Desired end-effector linear velocity (3,).
Return type:: np.ndarray

property w_ref: ndarray

Get the desired end-effector angular velocity.

Returns:: Desired end-effector angular velocity (3,).
Return type:: np.ndarray

property FT_ref: ndarray

Get the desired force/torque sensor data.

Returns:: Desired force/torque sensor data (6,).
Return type:: np.ndarray

property F_ref: ndarray

Get the desired force sensor data.

Returns:: Desired force sensor data (3,).
Return type:: np.ndarray

property Trq_ref: ndarray

Get the desired torque sensor data.

Returns:: Desired torque sensor data (3,).
Return type:: np.ndarray

property q_err: ndarray

Get the error in joint positions.

Returns:: Error in joint positions (nj,).
Return type:: np.ndarray

property qdot_err: ndarray

Get the error in joint velocities.

Returns:: Error in joint velocities (nj,).
Return type:: np.ndarray

property x_err: ndarray

Get the error in end-effector pose.

Returns:: Error in end-effector pose (7,).
Return type:: np.ndarray

property p_err: ndarray

Get the error in end-effector position.

Returns:: Error in end-effector position (3,).
Return type:: np.ndarray

property Q_err: ndarray

Get the error in end-effector quaternion.

Returns:: Error in end-effector quaternion (4,).
Return type:: np.ndarray

property R_err: ndarray

Get the error in end-effector rotation matrix.

Returns:: Error in end-effector rotation matrix (3, 3).
Return type:: np.ndarray

property T_err: ndarray

Get the error in end-effector transformation matrix.

Returns:: Error in end-effector transformation matrix (4, 4).
Return type:: np.ndarray

property v_err: ndarray

Get the error in end-effector velocity.

Returns:: Error in end-effector velocity (6,).
Return type:: np.ndarray

property pdot_err: ndarray

Get the error in end-effector linear velocity.

Returns:: Error in end-effector linear velocity (3,).
Return type:: np.ndarray

property w_err: ndarray

Get the error in end-effector angular velocity.

Returns:: Error in end-effector angular velocity (3,).
Return type:: np.ndarray

property J: ndarray

Get the Jacobian matric for current configuration.

Returns:: Jacobian matrix (6, nj).
Return type:: JacobianType

InitObject() → None[source]

Initializes the robot’s command and actual state with zeros, and sets default values for various attributes.

This method sets the initial values for joint positions, joint velocities, joint torques, end-effector pose, velocities, force/torque sensor data, control inputs, and other state variables.

Returns:: This method does not return any value. It modifies the internal state of the robot object.
Return type:: None

Init() → None[source]

Initializes the robot by setting up the object, state, current target, time, and logging a message.

This method calls the following functions to initialize the robot: 1. InitObject() - Initializes the command and actual states with default values. 2. GetState() - Retrieves the current state of the robot. 3. ResetCurrentTarget() - Resets the current target position. 4. ResetTime() - Resets the robot’s internal time.

Returns:: This method does not return any value. It modifies the robot’s internal state and logs a message.
Return type:: None

GetState() → None[source]

Abstract method to updates the robot’s state.

It has to be reimplemented in actual robot class!

It has to set: - Actual robot joint and task space states - Read all robot sensors. Robot states and signal are defined in robot frame except the task force/torque sensor data, which is defined in the nominal tool frame defned by CPGripper transformation.

This method sets the following attributes: - _tt: The current time, can be retrieved using simtime(). - _last_update: The last update time, retrieved using simtime().

Returns:: This method does not return any value. It modifies the internal state of the robot.
Return type:: None

Update() → None[source]

Updates the robot’s state and optionally triggers a capture callback.

This method performs the following actions:

If _do_update is True, it calls GetState() to update the robot’s internal state.
If _do_capture is True and a capture callback function (_capture_callback) is defined, it calls the callback function, passing the robot object as an argument.

Returns:: This method does not return any value. It modifies the internal state of the robot and may trigger a callback.
Return type:: None

EnableUpdate() → None[source]

Enables the update of the robot’s internal state.

This method sets the _do_update attribute to True, which allows the robot’s state to be updated.

Returns:: This method does not return any value. It modifies the internal state of the robot.
Return type:: None

DisableUpdate() → None[source]

Disables the update of the robot’s internal state.

This method sets the _do_update attribute to False, which prevents the robot’s state from being updated.

Returns:: This method does not return any value. It modifies the internal state of the robot.
Return type:: None

GetUpdateStatus() → bool[source]

Returns the current status of the update flag.

This method returns the value of the _do_update attribute, which indicates whether the robot’s state update is enabled.

Returns:: True if updates are enabled, False otherwise.
Return type:: bool

ResetCurrentTarget(do_move: bool = False, **kwargs: Any) → int[source]

Resets the current target for the robot and optionally moves the robot to the actual configuration.

This method updates the robot’s commanded joint positions, velocities, torques, and end-effector pose to the current actual values. It also has an optional move functionality that commands the robot to move to the updated target.

Parameters:

do_move (bool, optional) – If True, the robot will move to the actual configuration after resetting the target. Default is False.
**kwargs (Any) – Additional keyword arguments passed to the GoTo_T method when do_move is True.

Returns:

A status code: - 0 if no movement is performed (i.e., do_move=False), - 88 if the movement was not executed due to active threads, - The result of the GoTo_q or GoTo_T function otherwise.

Return type:

int

Notes

This method modifies the _command and _actual states of the robot.

ResetTaskTarget() → None[source]

Resets the task target for the robot based on the current commanded joint positions.

This method updates the robot’s commanded end-effector pose (_command.x) and velocity (_command.v) based on the robot’s current joint configuration.

Returns:: This method does not return any value but modifies the _command.x and _command.v attributes.
Return type:: None

GetJointPos(state: str | None = None, refresh: bool | None = None) → ndarray[source]

Get the joint positions of the robot based on the specified state.

Parameters:

state (str, optional) – The state from which to retrieve joint positions. Can be ‘Actual’ or ‘Commanded’. If None, the default state defined in the robot class is used.
refresh (bool, optional) – If True, the robot’s state is updated before retrieving joint positions. Default is True.

Returns:

The joint positions (q) from the specified state, copied to prevent external modifications.

Return type:

np.ndarray

Raises:

ValueError – If the state is not “Actual” or “Commanded”.

GetJointVel(state: str | None = None, refresh: bool | None = None) → ndarray[source]

Get the joint velocities of the robot based on the specified state.

Parameters:

state (str, optional) – The state from which to retrieve joint velocities. Can be ‘Actual’ or ‘Commanded’. If None, the default state defined in the robot class is used.
refresh (bool, optional) – If True, the robot’s state is updated before retrieving joint velocities. Default is True.

Returns:

The joint velocities (qdot) from the specified state, copied to prevent external modifications.

Return type:

np.ndarray

Raises:

ValueError – If the state is not “Actual” or “Commanded”.

GetJointTrq(state: str | None = None, refresh: bool | None = None) → ndarray[source]

Get the joint torques of the robot based on the specified state.

Parameters:

state (str, optional) – The state from which to retrieve joint torques. Can be ‘Actual’ or ‘Commanded’. If None, the default state defined in the robot class is used.
refresh (bool, optional) – If True, the robot’s state is updated before retrieving joint torques. Default is True.

Returns:

The joint torques (trq) from the specified state, copied to prevent external modifications.

Return type:

np.ndarray

Raises:

ValueError – If the state is not “Actual” or “Commanded”.

GetPose(out: str = None, task_space: str = None, kinematics: str = None, state: str = None, refresh: bool = None) → ndarray[source]

Get the robot’s end-effector pose.

Parameters:

out (str, optional) – Output form for the pose. The default is “x” (“Pose”).
task_space (str, optional) – Task space frame to use. Options are “World”, “Object”, and “Robot”. The default is “World”.
kinematics (str, optional) – The kinematics to use. Options are “Robot” or “Calculated”. The default is “Robot”.
state (str, optional) – The state of the robot to use. Options are “Actual” or “Commanded”. The default is “Actual”.
refresh (bool, optional) – If True, the robot’s state is updated before retrieving the pose. Default is True.

Returns:

The end-effector pose in the specified form, shape varies depending on out value.

Return type:

Pose3DType or HomogeneousMatrixType or Vector3DType or QuaternionType or RotationMatrixType

Raises:

ValueError – If the state, kinematics, or task_space options are invalid.

GetPos(out: str = 'p', task_space: str = None, kinematics: str = None, state: str = None) → ndarray[source]

Get the robot’s end-effector position.

Parameters:

out (str, optional) – Output form for the position. The default is “p” (“Position”).
task_space (str, optional) – Task space frame to use. Options are “World”, “Object”, and “Robot”. The default is “World”.
kinematics (str, optional) – The kinematics to use. Options are “Robot” or “Calculated”. The default is “Robot”.
state (str, optional) – The state of the robot to use. Options are “Actual” or “Commanded”. The default is “Actual”.

Returns:

The end-effector position (3,).

Return type:

Vector3DType

Raises:

ValueError – If the out form is not “Position” or “p”.

GetOri(out: str = 'Q', task_space: str = None, kinematics: str = None, state: str = None) → ndarray[source]

Get the robot’s end-effector orientation.

Parameters:

out (str, optional) – Output form for the orientation. Options are “Quaternion”, “Q”, “RotationMatrix”, “R”. The default is “Q” (“Quaternion”).
task_space (str, optional) – Task space frame to use. Options are “World”, “Object”, and “Robot”. The default is “World”.
kinematics (str, optional) – The kinematics to use. Options are “Robot” or “Calculated”. The default is “Robot”.
state (str, optional) – The state of the robot to use. Options are “Actual” or “Commanded”. The default is “Actual”.

Returns:

The end-effector orientation, either in quaternion (4,) or rotation matrix (3, 3) format.

Return type:

QuaternionType or RotationMatrixType

Raises:

ValueError – If the out form is not “Quaternion”, “Q”, “RotationMatrix”, or “R”.

GetVel(out: str = None, task_space: str = None, kinematics: str = None, state: str = None, refresh: bool = None) → ndarray[source]

Get robot end-effector velocity.

Parameters:

out (str, optional) – Output form for the velocity. Options are “Twist” (default), “Linear”, or “Angular”.
task_space (str, optional) – Task space frame for the velocity. Options are “World”, “Object”, “Tool”, or “Robot”. The default is “World”.
kinematics (str, optional) – The kinematics used for calculation. Options are “Robot” or “Calculated”. The default is “Robot”.
state (str, optional) – The state of the robot for the calculation. Options are “Actual” or “Commanded”. The default is “Actual”.
refresh (bool, optional) – If True, the robot’s state is updated before retrieving the velocity. Default is True.

Returns:

The end-effector velocity in the specified output form. The shape is either (6,) for the full twist or (3,) for linear or angular components.

Return type:

Velocity3DType or Vector3DType

Raises:

ValueError – If the out, task_space, or state values are not supported.

GetFT(out: str = None, source: str = None, task_space: str = 'FTFrame', kinematics: str = None, avg_time: int = 0, user_frame: ndarray | None = None, refresh: bool = None) → ndarray[source]

Get force/torque sensor data for the robot.

Parameters:

out (str, optional) – Output form for the force/torque data. Options are “Wrench” (default), “Force”, or “Torque”.
source (str, optional) – Source of the force/torque data. Options are “External” or “Robot”. The default is “Robot”.
task_space (str, optional) – Task space frame for the force/torque data. Options are “World”, “Object”, “Robot”, “Tool” or “TCP”, “FTFrame”, or “User”. The default is “Tool”.
kinematics (str, optional) – The kinematics used for the calculation. Options are “Robot” or “Calculated”. The default is “Robot”.
avg_time (int, optional) – Average time for the external force/torque sensor, by default 0.
user_frame (TCPType, optional) – User-defined frame for the force/torque data. Default is None.
refresh (bool, optional) – If True, the robot’s state is updated before retrieving the force/torque data. Default is True.

Returns:

The force/torque data in the specified output form. The shape varies depending on the out option.

Return type:

WrenchType or Vector3DType

Raises:

ValueError – If the source, task_space, or out values are not supported.

Notes

User defined frame user_frame represents the transforation from the robot flange

GoToActual(**kwargs: Any) → None[source]

Resets the current target and moves the robot to the actual configuration.

This method internally calls ResetCurrentTarget with do_move=True, which resets the current target configuration and moves the robot to the actual configuration.

Parameters:: **kwargs (Any) – Additional arguments passed to ResetCurrentTarget method.

GoTo_q(q: ndarray, qdot: ndarray | None = None, trq: ndarray | None = None, wait: float | None = None, **kwargs: Any) → None[source]

Abstract method to command the robot to go to a specific joint configuration.

It has to be reimplemented in actual robot class!

This method sets the commanded joint positions (q), velocities (qdot), and torques (trq), then sends them to the robot and waits for the specified time (wait).

Parameters:

q (JointConfigurationType) – Desired joint positions (nj,).
qdot (JointVelocityType, optional) – Desired joint velocities (nj,).
trq (JointTorqueType, optional) – Desired joint torques (nj,).
wait (float, optional) – Time to wait (in seconds) after commanding the robot to move.

GoTo_qtraj(q: ndarray, qdot: ndarray, qddotrq: ndarray, time: ndarray) → None[source]

Command the robot to follow a joint trajectory.

It has to be reimplemented in actual robot class!

It is intended to control the robot to follow a trajectory specified by joint positions (q), velocities (qdot), and accelerations (qddot) over a specified time (time).

Parameters:

q (JointConfigurationType) – Desired joint positions for the trajectory (n, nj), where n is the number of trajectory points.
qdot (JointVelocityType) – Desired joint velocities for the trajectory (n, nj), where n is the number of trajectory points.
qddotrq (JointAccelerationType) – Desired joint accelerations for the trajectory (n, nj), where n is the number of trajectory points.
time (TimesType) – Time points for the trajectory (n,).

Moves the robot to a specified joint position with specified velocity and trajectory.

Parameters:

q (JointConfigurationType) – Desired joint positions (nj,).
t (float, optional) – Time for the movement, by default None.
vel (float, optional) – Desired velocity for the movement, by default None.
vel_fac (JointConfigurationType, optional) – Velocity scaling factor for each joint, by default None.
wait (float, optional) – Time to wait after movement is completed, by default None.
traj (str, optional) – Trajectory type, by default None.
added_trq (JointTorqueType, optional) – Additional joint torques, by default None.
min_joint_dist (float, optional) – Minimum distance to target joint positions for movement execution, by default None.
asynchronous (bool, optional) – If True, executes the movement asynchronously, by default False.
**kwargs (Any) – Additional keyword arguments passed to internal methods.

Returns:

Thread – The thread executing the asynchronous movement, if asynchronous is True.
int – The status of the move (0 for success, 99 for abort) if asynchronous is False.

Notes

If asynchronous is set to True, the movement will be executed in a separate thread.
The control strategy should be set to “Joint” for this function to work.

Moves the robot by a specified joint displacement (dq) relative to the current joint positions.

Parameters:

dq (JointConfigurationType) – Joint displacement (nj,). The relative change in joint positions.
t (float, optional) – Time for the movement, by default None.
vel (float, optional) – Desired velocity for the movement, by default None.
vel_fac (JointConfigurationType, optional) – Velocity scaling factor for each joint, by default None.
state (str, optional) – State of the joint positions to reference, by default “Commanded”.
traj (str, optional) – Trajectory type, by default None.
wait (float, optional) – Time to wait after movement is completed, by default None.
added_trq (JointTorqueType, optional) – Additional joint torques, by default None.
asynchronous (bool, optional) – If True, executes the movement asynchronously, by default False.

Returns:

The status of the move (0 for success, 99 for abort).

Return type:

int

Notes

This function performs a joint move by adding the displacement (dq) to the current joint positions and then calls JMove to execute the movement.
If asynchronous is set to True, the movement will be executed in a separate thread.

Performs a linear joint trajectory move to the specified joint positions.

Parameters:

q (JointConfigurationType) – Desired joint positions (nj,).
t (float, optional) – Time for the movement, by default None.
vel (float, optional) – Desired velocity for the movement, by default None.
vel_fac (JointConfigurationType, optional) – Velocity scaling factor for each joint, by default None.
state (str, optional) – State of the joint positions to reference, by default “Commanded”.
wait (float, optional) – Time to wait after movement is completed, by default None.
added_trq (JointTorqueType, optional) – Additional joint torques, by default None.
asynchronous (bool, optional) – If True, executes the movement asynchronously, by default False.

Returns:

The status of the move (0 for success, 99 for abort).

Return type:

int

Notes

This function performs a joint move using a trapezoidal trajectory (“Trap”) to the desired joint positions (q).
If asynchronous is set to True, the movement will be executed in a separate thread.

JPath(path: ndarray, t: ndarray, wait: float | None = None, traj: str | None = None, added_trq: ndarray | None = None, asynchronous: bool | None = None, **kwargs: Any) → int[source]

Execute a joint path trajectory move.

Parameters:

path (JointPathType) – Joint path (n, nj) containing the waypoints to execute.
t (TimesType) – Time values associated with the path trajectory.
wait (float, optional) – Time to wait after movement. Default is None.
traj (str, optional) – Trajectory type. Default is None.
added_trq (JointTorqueType, optional) – Additional torques to apply during movement. Default is None.
asynchronous (bool, optional) – If True, execute the move asynchronously in a separate thread. Default is False.
**kwargs (Any) – Additional keyword arguments passed to internal methods.

Returns:

Status code of the move.

Return type:

int

Notes

This method moves the robot along a path defined by joint positions. If asynchronous is True, the movement is executed in a new thread.

JRBFPath(pathRBF: Dict[str, Any], t: float, direction: str = 'Forward', wait: float | None = None, traj: str | None = None, added_trq: ndarray | None = None, asynchronous: bool | None = None, **kwargs: Any) → int[source]

Execute a joint path generated from radial basis function interpolation.

Parameters:

pathRBF (Dict[str, Any]) – Dictionary containing the RBF parameters and control points.
t (float) – Time used to execute the path.
direction (str, optional) – Motion direction, either "Forward" or "Backward". Default is "Forward".
wait (float, optional) – Time to wait after the movement finishes. Default is None.
traj (str, optional) – Trajectory type. Default is None.
added_trq (JointTorqueType, optional) – Additional torques to apply during movement. Default is None.
asynchronous (bool, optional) – If True, execute the move in a separate thread. Default is False.
**kwargs (Any) – Additional keyword arguments passed to internal methods.

Returns:

Status code of the move.

Return type:

int

Notes

If asynchronous is True, the motion is executed in a new thread. The joint trajectory is computed from the control points stored in pathRBF.

GoTo_T(x: ndarray, v: ndarray | None = None, FT: ndarray | None = None, wait: float | None = None, **kwargs: Any) → int[source]

Move the robot to the target pose and velocity in Cartesian space.

Parameters:

x (Union[Pose3DType, HomogeneousMatrixType]) – Target end-effector pose in Cartesian space. Can be in different forms (e.g., pose or transformation matrix).
v (Velocity3DType, optional) – Target end-effector velocity in Cartesian space. Default is a zero velocity vector (6,).
FT (WrenchType, optional) – Target force/torque in Cartesian space. Default is a zero wrench vector (6,).
wait (float, optional) – The time to wait after the movement, by default the sample time (self.tsamp).
**kwargs (Any) – Additional keyword arguments passed to other methods, including task_space.

Returns:

The status of the move (0 for success, non-zero for error).

Return type:

int

Raises:

ValueError – If the provided task space is not supported.

Notes

The method first converts the input x, v, and FT based on the specified task space. The robot will be moved using either Cartesian control or a transformation control strategy.

Update task pose using cartesian space controller and wait

Parameters:

x (Union[Pose3DType, HomogeneousMatrixType]) – Target end-effector pose in Cartesian space. Can be in different forms (e.g., Pose, Transformation matrix).
v (Velocity3DType, optional) – Target end-effector velocity in Cartesian space. Default is a zero velocity vector (6,).
FT (WrenchType, optional) – WrenchType, optional, NOT USED! Target force/torque in Cartesian space. Default is a zero wrench vector (6,).
wait (float, optional) – The time to wait after the movement, by default the sample time (self.tsamp).
impedance (ArrayLike, optional) – The Cartesian impedance to set during the movement.
**kwargs (dict) – Additional keyword arguments for special use.
control. (The robot will be moved using Cartesian)

Returns:

Status of the move (0 for success, non-zero for error).

Return type:

int

Transforms Cartesian space trajectory to joiont space using inverse kinematics and then executes the trajectorj using GoTo_qtraj.

Parameters:

x (Union[Poses3DType, HomogeneousMatricesType]) – Target end-effector poses in Cartesian space.
t (TimesType) – Time vector for trajectory (n,).
wait (float, optional) – The time to wait after movement, by default None (using self.tsamp).
traj_samp_fac (float, optional) – The factor for trajectory sampling, by default None (using self._default.TrajSampTimeFac).
max_iterations (int, optional) – Maximum iterations for inverse kinematics, by default 1000.
pos_err (float, optional) – Position error tolerance, by default None (using self._default.PosErr).
ori_err (float, optional) – Orientation error tolerance, by default None (using self._default.OriErr).
task_space (str, optional) – The task space for the motion, by default None (using self._default.TaskSpace).
task_DOF (ArrayLike, optional) – Task degrees of freedom, by default None (using self._default.TaskDOF).
null_space_task (str, optional) – The null space task for optimization, by default None (using self._default.NullSpaceTask).
task_cont_space (str, optional) – The task control space, by default None (using self._default.TaskContSpace).
q_opt (JointConfigurationType, optional) – Optimal joint configuration, by default None (using self.q_home).
v_ns (Velocity3DType, optional) – Task velocity for null space, by default None (using zeros).
qdot_ns (JointConfigurationType, optional) – Joint velocity for null space, by default None (using zeros).
x_opt (Pose3DType, optional) – Optimal end-effector pose, by default None (calculated using self.Kinmodel(q_opt)).
Kp (float, optional) – Position control gain, by default None (using self._default.Kp).
Kns (float, optional) – Null space gain, by default None (using self._default.Kns).
state (str, optional) – The robot state, by default “Commanded”.
**kwargs (Any) – Additional keyword arguments for customization.

Returns:

Status of the operation: 0 for success, non-zero for failure.

Return type:

int

Raises:

ValueError – If the task space or kinematics calculation is not supported.

Notes

The method uses inverse kinematics to plan the trajectory from Cartesian to joint space. Cartesian movement is converted to joint space via IKin and IKinPath methods. If Cartesian movement is not feasible, a warning message is raised.

SetUserNSTaskCallback(fun: Callable[[...], Any]) → None[source]

Set the user callback function for null-space task.

Parameters:: fun (Callable[..., Any]) – The callback function to be called in GoTo_TC when null_space_task is set to ‘User’
Return type:: None

Kinematic controller for controlling robot end-effector in Cartesian space.

This function uses inverse kinematics to move the robot’s end-effector to a desired position while managing joint space motion through various null-space control strategies.

Parameters:

x (Union[Pose3DType, HomogeneousMatrixType]) – Target end-effector pose, expressed in the specified task space.
v (Velocity3DType, optional) – End-effector velocity (6,), by default None (zero velocity).
FT (WrenchType, optional) – End-effector Force/Torque (6,), by default None (zero force/torque).
timeout (float, optional) – Timeout for kinematic controller (by default 0).
pos_err (float, optional) – Position error tolerance, by default None (using self._default.PosErr).
ori_err (float, optional) – Orientation error tolerance, by default None (using self._default.OriErr).
task_space (str, optional) – The task space for motion, by default None (using self._default.TaskSpace).
task_DOF (ArrayLike, optional) – Task Degrees of Freedom, by default None (using self._default.TaskDOF).
null_space_task (str, optional) – Null-space task for optimization, by default None (using self._default.NullSpaceTask).
task_cont_space (str, optional) – Task control space, by default “Robot”.
q_opt (JointConfigurationType, optional) – Optimal joint configuration for null-space, by default None (using self.q_home).
v_ns (Velocity3DType, optional) – Task-space velocity, by default None (zero velocity).
qdot_ns (JointConfigurationType, optional) – Joint velocity for null-space control, by default None (zero joint velocity).
x_opt (Pose3DType, optional) – Optimal end-effector pose, by default None (calculated from self.Kinmodel(q_opt)).
Kp (float, optional) – Proportional gain for position control, by default None (using self._default.Kp).
Kns (float, optional) – Null-space gain, by default None (using self._default.Kns).
vel_fac (float, optional) – Velocity scaling factor for each joint, by default None.
**kwargs (Any) – Additional keyword arguments for flexibility.

Returns:

Status code (0 for success, non-zero for failure).

Return type:

int

Raises:

ValueError – If the task space or null-space task is not supported.

Notes

The method uses inverse kinematics to perform Cartesian motion to joint-space motion. Different null-space tasks can be applied for optimization such as manipulability, joint limits, etc.

Transforms Cartesian space trajectory to joiont space using inverse kinematics and then check if feasible.

Parameters:

x (CartesianPathType) – Target end-effector pose path in Cartesian space (n, 7) or (n, 4, 4).
t (TimesType) – Time vector for trajectory (n,).
q_init (JointConfigurationType) – initial joint configuration, by default None (using current configuration).
wait (float, optional) – The time to wait after movement, by default None (using self.tsamp).
traj_samp_fac (float, optional) – The factor for trajectory sampling, by default None (using self._default.TrajSampTimeFac).
max_iterations (int, optional) – Maximum iterations for inverse kinematics, by default 1000.
pos_err (float, optional) – Position error tolerance, by default None (using self._default.PosErr).
ori_err (float, optional) – Orientation error tolerance, by default None (using self._default.OriErr).
task_space (str, optional) – The task space for the motion, by default None (using self._default.TaskSpace).
task_DOF (ArrayLike, optional) – Task degrees of freedom, by default None (using self._default.TaskDOF).
null_space_task (str, optional) – The null space task for optimization, by default None (using self._default.NullSpaceTask).
task_cont_space (str, optional) – The task control space, by default None (using self._default.TaskContSpace).
q_opt (JointConfigurationType, optional) – Optimal joint configuration, by default None (using self.q_home).
v_ns (Velocity3DType, optional) – Velocity for null space, by default None (using zeros).
qdot_ns (JointVelocityType, optional) – Joint velocity for null space, by default None (using zeros).
x_opt (Pose3DType, optional) – Optimal end-effector pose, by default None (calculated using self.Kinmodel(q_opt)).
Kp (float, optional) – Position control gain, by default None (using self._default.Kp).
Kns (float, optional) – Null space gain, by default None (using self._default.Kns).
state (str, optional) – The robot state, by default “Commanded”.
**kwargs (Any) – Additional keyword arguments for customization.

Returns:

int: Status of the operation: 0 for success, non-zero for failure.
t_new: New time vesctor considering joint velocity limits

Return type:

list

Raises:

ValueError – If the task space or kinematics calculation is not supported.

Notes

The method uses inverse kinematics to plan the trajectory from Cartesian to joint space. Cartesian movement is converted to joint space via IKin and IKinPath methods. If Cartesian movement is not feasible, a warning message is raised.

Executes a Cartesian move with specified target position, velocity, and trajectory.

The robot moves its end-effector to a target position with optional velocity, trajectory type, and additional force/torque settings.

Parameters:

x (Pose3DType) – The target Cartesian pose (7,).
t (float, optional) – The duration for the movement, by default None.
vel (float, optional) – The velocity at which the end-effector moves, by default None.
vel_fac (Velocity3DType, optional) – A factor to scale the velocity, by default None.
traj (str, optional) – The trajectory type, by default None.
short (bool, optional) – Whether to shorten the path, by default None.
wait (float, optional) – The wait time after the movement, by default None.
task_space (str, optional) – The task space reference frame, by default None.
added_FT (WrenchType, optional) – Additional force/torque to be applied, by default None.
state (str, optional) – The state of the robot (e.g., “Commanded” or “Actual”), by default “Commanded”.
min_pos_dist (float, optional) – The minimum position distance to target positions for movement execution, by default None.
min_ori_dist (float, optional) – The minimum distance between actual and target orientation for movement execution, by default None.
asynchronous (bool, optional) – Whether the motion should be performed asynchronously, by default False.
**kwargs (Any) – Additional keyword arguments passed to internal methods.

Returns:

The status code of the move (0 if successful, non-zero if error occurred).

Return type:

int

Raises:

ValueError – If an unsupported task space or parameter shape is provided.

Move the robot in Cartesian space based on a displacement vector.

The robot moves its end-effector from the current position by a given displacement.

Parameters:

dx (ArrayLike) – Displacement in Cartesian space (3, ) for position or (3, 3) for rotation or (4, ) for quaternion.
t (float, optional) – Time to complete the movement, by default None.
vel (float, optional) – Maximum velocity, by default None.
vel_fac (ArrayLike, optional) – Velocity scaling factor, by default None.
traj (str, optional) – Trajectory type, by default None.
short (bool, optional) – Whether to shorten the trajectory, by default None.
wait (float, optional) – Wait time after movement, by default None.
task_space (str, optional) – The task space for the movement, by default None.
added_FT (WrenchType, optional) – Additional force/torque values, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, executes the motion asynchronously, by default False.
**kwargs (Any) – Additional keyword arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

Move the robot towards a target pose with an offset.

The robot moves to a position that is offset by a specified displacement from the given target pose.

Parameters:

x (Pose3DType) – The target pose (7, ) or (4, 4).
dx (Vector3DType) – The displacement to move towards (3, ).
t (float, optional) – Time to complete the movement, by default None.
vel (float, optional) – Maximum velocity, by default None.
vel_fac (ArrayLike, optional) – Velocity scaling factor, by default None.
traj (str, optional) – Trajectory type, by default None.
short (bool, optional) – Whether to shorten the trajectory, by default None.
wait (float, optional) – Wait time after movement, by default None.
task_space (str, optional) – The task space for the movement, by default None.
added_FT (WrenchType, optional) – Additional force/torque values, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, executes the motion asynchronously, by default False.
**kwargs (Any) – Additional keyword arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

Execute a linear move to a target Cartesian position.

The robot moves to the target position using a trapezoidal velocity profile.

Parameters:

x (Pose3DType) – The target pose (7,).
t (float, optional) – Time to complete the movement, by default None.
vel (float, optional) – Maximum velocity, by default None.
vel_fac (Velocity3DType, optional) – Velocity scaling factor, by default None.
short (bool, optional) – Whether to shorten the trajectory, by default None.
wait (float, optional) – Wait time after movement, by default None.
task_space (str, optional) – The task space for the movement, by default None.
added_FT (WrenchType, optional) – Additional force/torque values, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, executes the motion asynchronously, by default False.
**kwargs (Any) – Additional keyword arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

Execute a linear move based on a displacement vector.

The robot moves from its current position by the given displacement using a trapezoidal velocity profile.

Parameters:

dx (Pose3DType) – Displacement pose (7,).
t (float, optional) – Time to complete the movement, by default None.
vel (float, optional) – Maximum velocity, by default None.
vel_fac (Velocity3DType, optional) – Velocity scaling factor, by default None.
short (bool, optional) – Whether to shorten the trajectory, by default None.
wait (float, optional) – Wait time after movement, by default None.
task_space (str, optional) – The task space for the movement, by default None.
added_FT (WrenchType, optional) – Additional force/torque values, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, executes the motion asynchronously, by default False.
**kwargs (Any) – Additional keyword arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

Execute an arc trajectory movement in Cartesian space.

The robot moves its end-effector along an arc defined by a target pose and a center position.

Parameters:

x (Pose3DType) – Target position or pose (7, ) or (4, 4).
pC (Vector3DType) – Center of the arc in Cartesian space (3, ).
t (float, optional) – Time to complete the movement, by default None.
vel (float, optional) – Maximum velocity, by default None.
vel_fac (ArrayLike, optional) – Velocity scaling factor, by default None.
traj (str, optional) – Trajectory type, by default None.
short (bool, optional) – Whether to shorten the trajectory, by default None.
wait (float, optional) – Wait time after movement, by default None.
task_space (str, optional) – The task space for the movement, by default None.
added_FT (WrenchType, optional) – Additional force/torque values, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, executes the motion asynchronously, by default False.
**kwargs (Any) – Additional keyword arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

Execute a path trajectory movement in Cartesian space.

The robot moves its end-effector along a defined Cartesian path.

Parameters:

path (CartesianPathType) – Path in Cartesian space (n, 7) or (n, 4, 4) representing positions or poses.
t (Union[TimesType, float]) – Time to complete the movement, (n,) or scalar.
wait (float, optional) – Wait time after movement, by default None.
task_space (str, optional) – The task space for the movement, by default None.
added_FT (WrenchType, optional) – Additional force/torque values, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, executes the motion asynchronously, by default False.
**kwargs (Any) – Additional keyword arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

CRBFPath(pathRBF: Dict[str, Any], t: float, direction: str = 'Forward', wait: float = None, task_space: str = None, added_FT: ndarray | None = None, state: str = 'Commanded', asynchronous: bool | None = None, **kwargs: Any) → int[source]

Execute a path trajectory using Radial Basis Function (RBF) interpolation.

The robot moves along a path defined by a Radial Basis Function (RBF), with the option to move in the forward or backward direction.

Parameters:

pathRBF (Dict[str, Any]) – RBF path data containing control points and weights. The dictionary must include “c” for control points and “w” for weights.
t (float) – Total time for completing the path.
direction (str, optional) – Direction of motion, either “Forward” or “Backward”, by default “Forward”.
wait (float, optional) – Wait time after movement, by default None.
task_space (str, optional) – The task space for the movement, by default None.
added_FT (WrenchType, optional) – Additional force/torque values, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, executes the motion asynchronously, by default False.
**kwargs (Any) – Additional keyword arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

Jogging function for the robot using a space navigator.

This function allows the robot to jog (move) using a space navigator (3D input device). The robot’s movement is controlled based on the space navigator’s input, with scaling factors and different velocity modes. The user can toggle between position and orientation control, as well as change the velocity factor. The function terminates when both buttons on the space navigator are pressed simultaneously.

Parameters:

scale (ArrayLike, optional) – A scaling factor to adjust the velocity. If not provided, a default scaling factor of 10% of the robot’s maximum velocity is used. If a 2D vector is provided, it will be expanded into a 6D vector for position and orientation scaling. If a scalar is provided, it will be converted into a 6D vector.
null_space_task (str, optional) – The name of the null space task to be used by the robot. If not provided, the robot’s default null space task is used.

Returns:

Motion result code after jogging stops.

Return type:

int

Notes

The jogging functionality is toggled between position (translation) and orientation (rotation) by pressing the first button on the space navigator. The velocity factor can be toggled by pressing the second button.

The jogging mode is terminated when both buttons on the space navigator are pressed simultaneously.

Move the robot in tool space.

Parameters:

x (Pose3DType) – The target position/pose in tool space (7,) or (4, 4).
t (float, optional) – Time to complete the move, by default None.
vel (float, optional) – Linear velocity in m/s, by default None.
vel_fac (ArrayLike, optional) – Velocity scaling factor, by default None.
traj (str, optional) – Type of trajectory to use, by default None.
short (bool, optional) – If True, use a shorter trajectory, by default None.
wait (float, optional) – Wait time after move, by default None.
added_FT (WrenchType, optional) – Additional force/torque, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, the move is executed asynchronously, by default False.
**kwargs (Any) – Additional arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

Move the robot in tool space along a line.

Parameters:

x (Pose3DType) – The target position/pose in tool space (7,) or (4, 4).
t (float, optional) – Time to complete the move, by default None.
vel (float, optional) – Linear velocity in m/s, by default None.
vel_fac (ArrayLike, optional) – Velocity scaling factor, by default None.
short (bool, optional) – If True, use a shorter trajectory, by default None.
wait (float, optional) – Wait time after move, by default None.
added_FT (WrenchType, optional) – Additional force/torque, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, the move is executed asynchronously, by default False.
**kwargs (Any) – Additional arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

Move the robot in object space.

Parameters:

x (Pose3DType) – The target position/pose in object space (7,) or (4, 4).
t (float, optional) – Time to complete the move, by default None.
vel (float, optional) – Linear velocity in m/s, by default None.
vel_fac (ArrayLike, optional) – Velocity scaling factor, by default None.
traj (str, optional) – Type of trajectory to use, by default None.
short (bool, optional) – If True, use a shorter trajectory, by default None.
wait (float, optional) – Wait time after move, by default None.
added_FT (WrenchType, optional) – Additional force/torque, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, the move is executed asynchronously, by default False.
**kwargs (Any) – Additional arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

Move the robot in object space by a given displacement.

Parameters:

dx (Vector3DType) – The displacement vector in object space (3,).
t (float, optional) – Time to complete the move, by default None.
vel (float, optional) – Linear velocity in m/s, by default None.
vel_fac (ArrayLike, optional) – Velocity scaling factor, by default None.
traj (str, optional) – Type of trajectory to use, by default None.
short (bool, optional) – If True, use a shorter trajectory, by default None.
wait (float, optional) – Wait time after move, by default None.
added_FT (WrenchType, optional) – Additional force/torque, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, the move is executed asynchronously, by default False.
**kwargs (Any) – Additional arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

Approach the target in object space.

Parameters:

x (Pose3DType) – The target position/pose in object space (7,) or (4, 4).
dx (Vector3DType) – The displacement vector in object space (3,).
t (float, optional) – Time to complete the approach, by default None.
vel (float, optional) – Linear velocity in m/s, by default None.
vel_fac (ArrayLike, optional) – Velocity scaling factor, by default None.
traj (str, optional) – Type of trajectory to use, by default None.
short (bool, optional) – If True, use a shorter trajectory, by default None.
wait (float, optional) – Wait time after approach, by default None.
added_FT (WrenchType, optional) – Additional force/torque, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, the approach is executed asynchronously, by default False.
**kwargs (Any) – Additional arguments passed to internal methods.

Returns:

Status code of the approach (0 if successful, non-zero if failed).

Return type:

int

Move the robot in object space along a line.

Parameters:

x (Pose3DType) – The target position/pose in object space (7,) or (4, 4).
t (float, optional) – Time to complete the move, by default None.
vel (float, optional) – Linear velocity in m/s, by default None.
vel_fac (ArrayLike, optional) – Velocity scaling factor, by default None.
short (bool, optional) – If True, use a shorter trajectory, by default None.
wait (float, optional) – Wait time after move, by default None.
added_FT (WrenchType, optional) – Additional force/torque, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, the move is executed asynchronously, by default False.
**kwargs (Any) – Additional arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

Move the robot in object space by a given displacement along a line.

Parameters:

x (Pose3DType) – The target position/pose in object space (7,) or (4, 4).
dx (Vector3DType) – The displacement vector in object space (3,).
t (float, optional) – Time to complete the move, by default None.
vel (float, optional) – Linear velocity in m/s, by default None.
vel_fac (ArrayLike, optional) – Velocity scaling factor, by default None.
short (bool, optional) – If True, use a shorter trajectory, by default None.
wait (float, optional) – Wait time after move, by default None.
added_FT (WrenchType, optional) – Additional force/torque, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, the move is executed asynchronously, by default False.
**kwargs (Any) – Additional arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

Move the robot in object space along an arc.

Parameters:

x (Pose3DType) – The target position/pose in object space (7,) or (4, 4).
pC (Vector3DType) – The center of the arc (3,).
t (float, optional) – Time to complete the move, by default None.
vel (float, optional) – Linear velocity in m/s, by default None.
vel_fac (ArrayLike, optional) – Velocity scaling factor, by default None.
traj (str, optional) – Type of trajectory to use, by default None.
short (bool, optional) – If True, use a shorter trajectory, by default None.
wait (float, optional) – Wait time after move, by default None.
added_FT (WrenchType, optional) – Additional force/torque, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, the move is executed asynchronously, by default False.
**kwargs (Any) – Additional arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

OPath(path: ndarray, t: float, wait: float | None = None, task_space: str | None = None, added_FT: ndarray | None = None, state: str = 'Commanded', asynchronous: bool | None = None, **kwargs: Any) → int[source]

Move the robot along a path in object space.

Parameters:

path (CartesianPathType) – The path to follow in object space (n, 7) or (n, 4, 4).
t (float) – Time to complete the path move.
wait (float, optional) – Wait time after move, by default None.
task_space (str, optional) – Task space frame, by default None.
added_FT (WrenchType, optional) – Additional force/torque, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, the move is executed asynchronously, by default False.
**kwargs (Any) – Additional arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

ORBFPath(pathRBF: Dict[str, Any], t: float, direction: str = 'Forward', wait: float = None, task_space: str = None, added_FT: ndarray | None = None, state: str = 'Commanded', asynchronous: bool | None = None, **kwargs: Any) → int[source]

Move the robot along a Radial Basis Function (RBF) path in object space.

Parameters:

pathRBF (Dict[str, Any]) – The RBF path data, including control points and weights.
t (float) – Time to complete the path move.
direction (str, optional) – Direction of the path, “Forward” or “Backward”, by default “Forward”.
wait (float, optional) – Wait time after move, by default None.
task_space (str, optional) – Task space frame, by default None.
added_FT (WrenchType, optional) – Additional force/torque, by default None.
state (str, optional) – The robot state (“Commanded” or “Actual”), by default “Commanded”.
asynchronous (bool, optional) – If True, the move is executed asynchronously, by default False.
**kwargs (Any) – Additional arguments passed to internal methods.

Returns:

Status code of the move (0 if successful, non-zero if failed).

Return type:

int

AvailableStrategies() → list[str][source]

Get the available control strategies for the robot.

Returns:: List of available control strategies, by default includes the current control strategy.
Return type:: list[str]

SetStrategy(strategy: str) → None[source]

Set the control strategy for the robot.

Parameters:: strategy (str) – The control strategy to set for the robot.

GetStrategy() → str[source]

Get the current control strategy of the robot.

Returns:: The current control strategy of the robot.
Return type:: str

isStrategy(strategy: str) → bool[source]

Check if the current control strategy matches the provided strategy.

Parameters:: strategy (str) – The control strategy to check against the current strategy.
Returns:: True if the current strategy matches the provided strategy, False otherwise.
Return type:: bool

GetJointStiffness() → ndarray[source]

Get the joint stiffness of the robot.

Returns:: An array representing the joint stiffness, defaulting to a high stiffness value.
Return type:: JointConfigurationType

SetJointStiffness(stiffness: ndarray, **kwargs: Any) → None[source]

Set the joint stiffness for the robot.

Parameters:

stiffness (JointConfigurationType) – The stiffness values for the robot’s joints.
**kwargs (Any) – Additional arguments passed to the method.

GetJointDamping() → ndarray[source]

Get the joint damping of the robot.

Returns:: An array representing the joint damping, defaulting to a value of 1 for each joint.
Return type:: JointConfigurationType

SetJointDamping(damping: ndarray, **kwargs: Any) → None[source]

Set the joint damping for the robot.

Parameters:

damping (JointConfigurationType) – The damping values for the robot’s joints.
**kwargs (Any) – Additional arguments passed to the method.

SetJointSoft(softness: float, **kwargs: Any) → None[source]

Set the joint softness for the robot.

Parameters:

softness (float) – The softness value for the robot’s joints.
**kwargs (Any) – Additional arguments passed to the method.

SetJointStiff() → None[source]

Set the joints to be stiff (high stiffness).

This is a shorthand for setting joint stiffness to 1.0.

SetJointCompliant() → None[source]

Set the joints to be compliant (low stiffness).

This is a shorthand for setting joint stiffness to 0.0.

GetCartesianStiffness() → ndarray[source]

Get the Cartesian stiffness of the robot.

Returns:: An array representing the Cartesian stiffness, defaulting to a high stiffness value.
Return type:: Velocity3DType

SetCartesianStiffness(stiffness: ndarray, **kwargs: Any) → None[source]

Set the Cartesian stiffness for the robot.

Parameters:

stiffness (Velocity3DType) – The stiffness values for the robot’s Cartesian space.
**kwargs (Any) – Additional arguments passed to the method.

GetCartesianDamping() → ndarray[source]

Get the Cartesian damping of the robot.

Returns:: An array representing the Cartesian damping, defaulting to a value of 1 for each component.
Return type:: Velocity3DType

SetCartesianDamping(damping: ndarray, **kwargs: Any) → None[source]

Set the Cartesian damping for the robot.

Parameters:

damping (Velocity3DType) – The damping values for the robot’s Cartesian space.
**kwargs (Any) – Additional arguments passed to the method.

SetCartesianSoft(softness: float, **kwargs: Any) → None[source]

Set the Cartesian softness for the robot.

Parameters:

softness (float) – The softness value for the robot’s Cartesian space.
**kwargs (Any) – Additional arguments passed to the method.

SetCartesianStiff() → None[source]

Set the robot’s Cartesian space to be stiff (high stiffness).

This is a shorthand for setting Cartesian stiffness to 1.0.

SetCartesianCompliant() → None[source]

Set the robot’s Cartesian space to be compliant (low stiffness).

This is a shorthand for setting Cartesian stiffness to 0.0.

SetTeachMode() → None[source]

Set the robot control to Tesch mode.

BY default is sets the compliance to 0

EndTeachMode() → None[source]

End the robot control to Tesch mode.

By default is sets the compliance to default value

Map from robot base frame to world frame.

Supported arguments: pose (7,), Homogenous matrix (4, 4), rotation matrix (3, 3), position (3,), twist (6,) and JacobianType (6, nj).

Parameters:

x (ArrayLike) – Argument to map. It can be one of the following shapes: - pose (7,) or (4, 4) - position (3,) - orientation (4,) or (3, 3) - velocity or force (6,) - JacobianType (6, nj)
typ (str, optional) – Transformation type, by default None. If “Wrench”, the transformation considers the force. If “Twist”, the transformation considers the velocity.

Returns:

Mapped argument in the world frame.

Return type:

np.ndarray

Raises:

ValueError – If the parameter shape is not supported.

Map from world frame to robot base frame.

Supported arguments: pose (7,), Homogenous matrix (4, 4), rotation matrix (3, 3), position (3,), twist (6,) and JacobianType (6, nj).

Parameters:

x (ArrayLike) – Argument to map. It can be one of the following shapes: - pose (7,) or (4, 4) - position (3,) - orientation (4,) or (3, 3) - velocity or force (6,) - JacobianType (6, nj)
typ (str, optional) – Transformation type, by default None. If “Wrench”, the transformation considers the force. If “Twist”, the transformation considers the velocity.

Returns:

Mapped argument in the robot base frame.

Return type:

np.ndarray

Raises:

ValueError – If the parameter shape is not supported.

Map from object frame to world frame.

Supported arguments: pose (7,), Homogenous matrix (4, 4), rotation matrix (3, 3), position (3,), twist (6,) and JacobianType (6, nj).

Parameters:

x (ArrayLike) – Argument to map. It can be one of the following shapes: - pose (7,) or (4, 4) - position (3,) - orientation (4,) or (3, 3) - velocity or force (6,) - JacobianType (6, nj)
typ (str, optional) – Transformation type, by default None. If “Wrench”, the transformation considers the force. If “Twist”, the transformation considers the velocities.

Returns:

Mapped argument in the world frame.

Return type:

np.ndarray

Raises:

ValueError – If the parameter shape is not supported.

Map from world frame to object frame.

Supported arguments: pose (7,), Homogenous matrix (4, 4), rotation matrix (3, 3), position (3,), twist (6,) and JacobianType (6, nj).

Parameters:

x (ArrayLike) – Argument to map. It can be one of the following shapes: - pose (7,) or (4, 4) - position (3,) - orientation (4,) or (3, 3) - velocity or force (6,) - JacobianType (6, nj)
typ (str, optional) – Transformation type, by default None. If “Wrench”, the transformation considers the force. If “Twist”, the transformation considers the velocities.

Returns:

Mapped argument in the object frame.

Return type:

np.ndarray

Raises:

ValueError – If the parameter shape is not supported.

Map a variable from the camera frame to the robot base frame.

The camera pose in the robot base frame is computed as Tflange @ CameraFrame, where Tflange is the current robot flange pose in the robot base frame and CameraFrame is the camera pose relative to the flange.

Parameters:

x (ArrayLike) – Variable expressed in the camera frame. Supported shapes follow robotblockset.transformations.frame2world(), including poses, homogeneous transforms, positions, rotations, twists, and wrenches.
typ (str, optional) – Transformation type, by default None. Use "Twist" or "Wrench" for spatial velocity or force/torque variables.
kinematics (str, optional) – Kinematics source for the current robot pose, by default None.
state (str, optional) – Robot state for the current robot pose, by default None.
refresh (bool, optional) – Whether to refresh robot state before reading the pose, by default None.

Returns:

Mapped variable in the robot base frame.

Return type:

np.ndarray

Raises:

ValueError – If CameraFrame is not set.

Map a variable from the robot base frame to the camera frame.

The camera pose in the robot base frame is computed as Tflange @ CameraFrame, where Tflange is the current robot flange pose in the robot base frame and CameraFrame is the camera pose relative to the flange.

Parameters:

x (ArrayLike) – Variable expressed in the robot base frame. Supported shapes follow robotblockset.transformations.world2frame(), including poses, homogeneous transforms, positions, rotations, twists, and wrenches.
typ (str, optional) – Transformation type, by default None. Use "Twist" or "Wrench" for spatial velocity or force/torque variables.
kinematics (str, optional) – Kinematics source for the current robot pose, by default None.
state (str, optional) – Robot state for the current robot pose, by default None.
refresh (bool, optional) – Whether to refresh robot state before reading the pose, by default None.

Returns:

Mapped variable in the camera frame.

Return type:

np.ndarray

Raises:

ValueError – If CameraFrame is not set.

abstractmethod Kinmodel(*q: ndarray, tcp: ndarray | None = None, out: str = 'x') → Tuple[ndarray, ndarray] | Tuple[ndarray, ndarray, ndarray][source]

Abstract method to compute the forward kinematics of the robot.

Parameters:

*q (tuple) – Joint angles as input to the kinematic model.
tcp (TCPType, optional) – Tool Center Point (TCP) pose or transformation, by default None.
out (str, optional) – Output format for the result (pose, position, etc.), by default “x”.

Returns:

Pose representation and JacobianType, depending on out.

Return type:

tuple

DKin(q: ndarray | None = None, out: str | None = None, task_space: str | None = None) → ndarray[source]

Compute the direct kinematics (position and orientation) for given joint angles.

Parameters:

q (JointConfigurationType, optional) – Joint positions (nj,).
out (str, optional) – Output format for the result, by default None (depends on robot settings).
task_space (str, optional) – The task space frame to use for the result. Can be “World”, “Object”, or “Robot”, by default None.

Returns:

The computed task position (7,) or transformation matrix (4, 4), depending on the output format.

Return type:

Pose3DType or HomogeneousMatrixType

DKinPath(path: ndarray, out: str | None = None) → ndarray[source]

Compute direct kinematics for a path of joint positions.

Parameters:

path (JointPathType) – Path in joint space - poses (n, nj).
out (str, optional) – Output format for the result, by default None (depends on robot settings).

Returns:

Task positions at target pose (n, 7) or (4, 4, n), depending on the output format.

Return type:

Poses3DType or HomogeneousMatrixArrayType

Compute inverse kinematics to find joint positions that achieve a target Cartesian pose.

Parameters:

x (Union[Pose3DType, HomogeneousMatrixType]) – Target Cartesian pose (7,) or (4, 4).
q0 (JointConfigurationType, optional) – Initial joint positions (nj,). Defaults to current joint positions if not provided.
max_iterations (int, optional) – Maximum number of iterations for the inverse kinematics algorithm, by default 1000.
pos_err (float, optional) – Position error tolerance, by default None (uses default value).
ori_err (float, optional) – Orientation error tolerance, by default None (uses default value).
task_space (str, optional) – Task space in which to perform the inverse kinematics, by default “Robot”.
task_DOF (ArrayLike, optional) – Degrees of freedom of the task, by default None.
null_space_task (str, optional) – Type of null-space task, by default None.
task_cont_space (str, optional) – The space in which task continuity is considered, by default “Robot”.
q_opt (JointConfigurationType, optional) – Optimal joint positions for null space tasks, by default None.
v_ns (Velocity3DType, optional) – Null-space velocity for task-space velocity, by default None.
qdot_ns (JointVelocityType, optional) – Null-space joint velocities, by default None.
x_opt (Pose3DType, optional) – Optimal Cartesian pose for null-space tasks, by default None.
Kp (float, optional) – Proportional gain for the controller, by default None.
Kns (float, optional) – Gain for the null-space task, by default None.
save_path (bool, optional) – Whether to save the joint positions for the path, by default False.

Returns:

Joint positions at target pose (nj,) or joint path when save_path is True, and a status code.

Return type:

tuple

Compute inverse kinematics for a path of Cartesian poses.

Parameters:

path (CartesianPathType) – Path in Cartesian space - poses (n,7) or (n,4,4).
q0 (JointConfigurationType) – Initial joint positions (nj,).
max_iterations (int, optional) – Maximum number of iterations, by default 100.
pos_err (float, optional) – Position error tolerance, by default None (uses default).
ori_err (float, optional) – Orientation error tolerance, by default None (uses default).
task_space (str, optional) – Task space in which to compute the inverse kinematics, by default “Robot”.
task_DOF (ArrayLike, optional) – Degrees of freedom for the task, by default None.
null_space_task (str, optional) – Type of null-space task, by default None.
task_cont_space (str, optional) – Task continuity space, by default “Robot”.
q_opt (JointConfigurationType, optional) – Optimal joint positions for null-space tasks, by default None.
v_ns (Velocity3DType, optional) – Null-space velocity for task-space velocity, by default None.
qdot_ns (JointVelocityType, optional) – Null-space joint velocities, by default None.
x_opt (Pose3DType, optional) – Optimal Cartesian pose for null-space tasks, by default None.
Kp (float, optional) – Proportional gain, by default None.
Kns (float, optional) – Gain for null-space tasks, by default None.

Returns:

Joint positions at target pose for each point in the path (n, nj) and a status code.

Return type:

tuple

Jacobi(q: ndarray | None = None, tcp: ndarray | None = None, task_space: str = 'Robot') → ndarray[source]

Compute the Jacobian matrix for the robot given joint positions.

Parameters:

q (JointConfigurationType, optional) – Joint positions (nj,). If not provided, the actual joint positions are used.
tcp (TCPType, optional) – by default the value of self.TCP is used.
task_space (str, optional) – The task space in which to compute the Jacobian, by default “Robot”.

Returns:

Jacobian matrix of shape (6, nj) or (6, n), where nj is the number of joints.

Return type:

JacobianType

Compute the manipulability measure of the robot at a given joint configuration.

Parameters:

q (JointConfigurationType, optional) – Joint positions (nj,).
task_space (str, optional) – The task space frame to use for the Jacobian, by default “Robot”.
task_DOF (ArrayLike, optional) – Task Degrees of Freedom (DOF) for the manipulability calculation, by default all 6 DOF are considered.

Returns:

Manipulability measure, which is the square root of the determinant of the Jacobian matrix.

Return type:

float

JointDistance(q: ndarray, state: str = 'Actual') → ndarray[source]

Calculate the distance between the current joint position and a target joint position.

Parameters:

q (JointConfigurationType) – Joint positions (nj,) to calculate the distance from.
state (str, optional) – The state of the joint positions, by default “Actual”. Other options are “Command” for commanded joint positions.

Returns:

Distance (nj,) between the current joint position and the target joint position q.

Return type:

JointConfigurationType

TaskDistance(x: ndarray, out: str = 'x', task_space: str = 'World', state: str = 'Actual', kinematics: str = 'Calculated') → ndarray[source]

Calculate the distance between the current pose and a target pose.

Parameters:

x (Pose3DType) – The target pose to compare to the current pose.
out (str, optional) – The output form of the distance, by default “x”. Possible values are “x”, “p”, and “Q”.
task_space (str, optional) – The task space to use for the pose transformation, by default “World”.
state (str, optional) – The state of the current pose, by default “Actual”. Other options include “Command” for commanded poses.
kinematics (str, optional) – The type of kinematics to use, by default “Calculated”. Other options might include “Robot”.

Returns:

The distance between the current and target poses.

Return type:

Pose3DType or Vector3DType or QuaternionType

CheckJointLimits(q: ndarray) → bool[source]

Check if the joint positions q are within the joint range.

Parameters:: q (JointConfigurationType) – Joint position (nj,).
Returns:: True if one or more joints are out of the limits, False otherwise.
Return type:: bool

DistToJointLimits(q: ndarray | None = None) → Tuple[ndarray, ndarray, ndarray][source]

Calculate the distance to the joint limits.

Parameters:

q (JointConfigurationType, optional) – Joint position (nj,), by default None. If None, uses the current joint positions.

Returns:

Minimal distance to joint limits (nj,).
Distance to lower joint limits (nj,).
Distance to upper joint limits (nj,).

Return type:

tuple

SetGripper(gripper: Any | None = None) → None[source]

Attach or detach a gripper to the robot.

Parameters:: gripper (Any, optional) – The gripper to be attached to the robot, by default None. If None, the current gripper will be detached.

GetGripper() → list[Any][source]

Get the current gripper and its name.

Returns:: A list where the first element is the current gripper (None if not set) and the second element is the gripper’s name, or “None” if no gripper is set.
Return type:: list[Any]

SetCamera(camera: Any | None = None) → None[source]

Attach or detach a camera to the robot.

Parameters:: camera (Any, optional) – The camera to be attached to the robot, by default None. If None, the current camera will be detached.

GetCamera() → list[Any][source]

Get the current camera and its name.

Returns:: A list where the first element is the current camera (None if not set) and the second element is the camera’s name, or “None” if no camera is set.
Return type:: list[Any]

SetCameraFrame(x: ndarray) → None[source]

Set the transformation matrix or pose of the camera frame in robot flange frame.

Parameters:: x (TCPType) – The pose or transformation of the camera frame. It can be a 4x4 homogeneous matrix, a 3x3 rotation matrix, or any compatible representation.

GetCameraFrame(out: str | None = None) → ndarray | None[source]

Get the current camera frame in the specified output format.

Parameters:: out (str, optional) – The output form, by default None. The format can be “x”, “p”, “Q”, or other valid forms.
Returns:: The current camera frame in the specified output format.
Return type:: Optional[Union[Pose3DType, HomogeneousMatrixType, Vector3DType, QuaternionType, RotationMatrixType]]

SetFTFrame(FTFrame: ndarray | None = None) → None[source]

Set the F/T frame.

Parameters:: FTFrame (TCPType, optional) – The transformation matrix or pose of the F/T frame. Default is the identity matrix.
Return type:: None

GetFTFrame() → ndarray[source]

Get the current FT frame

Parameters:: out (str, optional) – The output form, by default None. The format can be “x”, “p”, “Q”, or other valid forms.
Returns:: The current FT frame.
Return type:: HomogeneousMatrixType

GetFTFramePose(out: str | None = None, task_space: str | None = None) → ndarray[source]

Get the current FT frame pose in the specified task space and output format.

Parameters:

out (str, optional) – The output form, by default None. The format can be “x”, “p”, “Q”, or other valid forms.
task_space (str, optional) – The task space for the pose transformation, by default “World”. Other options can be “Object”, “Robot”, or “Tool”.

Returns:

The current FT sensor pose in the specified task space and output form.

Return type:

Pose3DType or HomogeneousMatrixType or Vector3DType or QuaternionType or RotationMatrixType

SetFTSensor(FTsensor: Any | None = None) → None[source]

Attach or detach a force/torque (FT) sensor to the robot.

Parameters:: FTsensor (Any, optional) – The FT sensor to be attached to the robot, by default None. If None, the current FT sensor will be detached.

GetFTSensor() → list[Any][source]

Get the current FT sensor and its name.

Returns:: A list where the first element is the current FT sensor (None if not set) and the second element is the FT sensor’s name, or “None” if no FT sensor is set.
Return type:: list[Any]

SetFTSensorFrame(x: ndarray) → None[source]

Set the transformation matrix or pose of the FT sensor frame in robot flange frame.

Parameters:: x (TCPType) – The pose or transformation of the FT sensor frame. It can be a 4x4 homogeneous matrix, a 3x3 rotation matrix, or any compatible representation.

GetFTSensorFrame(out: str | None = None) → ndarray | None[source]

Get the current FT sensor frame in the specified output format.

Parameters:: out (str, optional) – The output form, by default None. The format can be “x”, “p”, “Q”, or other valid forms.
Returns:: The current FT sensor frame in the specified output form.
Return type:: Pose3DType or HomogeneousMatrixType or Vector3DType or QuaternionType or RotationMatrixType, optional

GetFTSensorPose(out: str | None = None, task_space: str | None = None) → ndarray[source]

Get the current FT sensor pose in the specified task space and output format.

Parameters:

out (str, optional) – The output form, by default None. The format can be “x”, “p”, “Q”, or other valid forms.
task_space (str, optional) – The task space for the pose transformation, by default “World”. Other options can be “Object” or “Robot”.

Returns:

The current FT sensor pose in the specified task space and output form.

Return type:

Pose3DType or HomogeneousMatrixType or Vector3DType or QuaternionType or RotationMatrixType

Set the load properties of the FT sensor.

Parameters:

load (load_params, optional) – The load object, by default None.
mass (float, optional) – The mass of the load, by default None.
COM (Vector3DType, optional) – The center of mass of the load, by default None.
inertia (np.ndarray, optional) – The inertia of the load, by default None.
offset (np.ndarray, optional) – The offset of the load, by default None.

GetFTSensorLoad() → load_params | None[source]

Get the load properties of the FT sensor.

Returns:: The current load properties of the FT sensor, or None if no load is set.
Return type:: Load or None

GetContacts(**kwargs) → ndarray | None[source]

Return contact forces.

THis method is intended to be overridden by derived classes that implement specific contact sensing capabilities. In the base class, it simply returns None.

Parameters:: **kwargs (Any) – Additional keyword arguments passed to internal methods.
Returns:: What is returned depends on the implementation. In the base class, this method returns None, but in derived classes it may return contact information (force of something else)
Return type:: None

SetLoad(load: load_params | None = None, mass: float | None = None, COM: ndarray | None = None, inertia: ndarray | None = None) → None[source]

Set the load properties of the robot.

Parameters:

load (load_params, optional) – The load object to be assigned, by default None.
mass (float, optional) – The mass of the load, by default None.
COM (Vector3DType, optional) – The center of mass of the load, by default None.
inertia (np.ndarray, optional) – The inertia of the load, by default None.

GetLoad() → load_params | None[source]

Get the current load properties of the robot.

Returns:: The current load object, or None if no load is set.
Return type:: load_params or None

SelectToolFromYAML(tool_name: str | None = None, tool_yaml_file: str | Path | None = 'tools.yaml') → None[source]

Apply tool TCP and load parameters to a robot instance.

Parameters:

tool_name (str, optional) – Tool name.
tool_yaml_file (Union[str, Path], optional) – str | Path, optional Path to the YAML file containing tool definitions. Defauls to “tools.yaml”

Return type:

None

SetTCP(tcp: ndarray | None = None, frame: str = 'Gripper') → None[source]

Set the Tool Center Point (TCP) of the robot.

Parameters:

tcp (TCPType, optional) – The transformation matrix or pose of the TCP. Default is the identity matrix.
frame (str, optional) – The frame to which the TCP is referenced. Can be “Gripper” or “Flange”. Default is “Gripper”.

Return type:

None

GetTCP(out: str = 'T') → ndarray[source]

Get the Tool Center Point (TCP) of the robot.

Parameters:: out (str, optional) – The output format of the TCP. Default is “T” (transformation matrix).
Returns:: The TCP in the specified output format.
Return type:: Pose3DType or HomogeneousMatrixType or Vector3DType or QuaternionType or RotationMatrixType

SetTCPGripper(tcp: ndarray | None = None) → None[source]

Set the TCP for the gripper.

Parameters:: tcp (TCPType, optional) – The transformation matrix or pose of the gripper TCP. Default is the identity matrix.
Return type:: None

GetTCPGripper(out: str = 'T') → ndarray[source]

Get the Tool Center Point (TCP) of the gripper.

Parameters:: out (str, optional) – The output format of the gripper TCP. Default is “T” (transformation matrix).
Returns:: The gripper TCP in the specified output format.
Return type:: Pose3DType or HomogeneousMatrixType or Vector3DType or QuaternionType or RotationMatrixType

SetObject(x: ndarray | None = None) → None[source]

Set the object pose in the robot’s coordinate system.

Parameters:: x (Union[Pose3DType, HomogeneousMatrixType], optional) – The pose of the object (7,) or (4, 4) (default is None, which sets to the actual object pose).
Return type:: None

GetObject(out: str = 'T', task_space: str | None = None) → ndarray[source]

Get the object pose in the specified task space.

Parameters:

out (str, optional) – The output format of the object pose. Default is “T” (transformation matrix).
task_space (str, optional) – The task space for the pose transformation. Can be “World”, “Object”, or “Robot”. Default is None.

Returns:

The object pose in the specified output format.

Return type:

Pose3DType or HomogeneousMatrixType or Vector3DType or QuaternionType or RotationMatrixType

Raises:

ValueError – If the task space is not recognized or the output format is not supported.

SetBasePose(x: ndarray) → None[source]

Set the robot base pose.

Parameters:: x (Union[Pose3DType, HomogeneousMatrixType]) – The pose of the base (7,) or (4, 4).
Return type:: None
Raises:: ValueError – If the base pose shape is not recognized.

GetBasePose(out: str = 'T') → ndarray[source]

Get the robot base pose.

Parameters:: out (str, optional) – The output format of the base pose. Default is “T” (transformation matrix).
Returns:: The base pose in the specified output format.
Return type:: Pose3DType or HomogeneousMatrixType or Vector3DType or QuaternionType or RotationMatrixType

SetBaseVel(v: ndarray) → None[source]

Set the robot base velocity.

Parameters:: v (Velocity3DType) – The velocity of the base (6,).
Return type:: None
Raises:: ValueError – If the base pose shape is not recognized.

GetBaseVel() → ndarray[source]

Get the robot base velocity.

Returns:: The base velocity.
Return type:: Velocity3DType

SetBasePlatform(platform: Any, x: ndarray | None = None) → None[source]

Set the base platform and optionally set its pose.

Parameters:

platform (Any) – The platform to attach to the robot.
x (Union[Pose3DType, HomogeneousMatrixType], optional) – The pose to set for the base platform (7,) or (4, 4).

Return type:

None

GetBasePlatform() → list[Any][source]

Get the current base platform attached to the robot.

Returns:: A list containing the platform object and its name. If no platform is attached, returns [None, “None”].
Return type:: list[Any]

UpdateRobotBase() → ndarray[source]

Update the robot base pose from the base platform, if available.

Returns:: The updated base pose.
Return type:: HomogeneousMatrixType

Start() → bool[source]

Start the robot’s motion by setting the control mode to 0.5 and resetting error states.

Returns:: True if the robot is active and motion can start, False otherwise.
Return type:: bool

Stop() → None[source]

Stop the robot’s motion by setting the control mode to 0 and resetting velocities, errors and threads.

Return type:: None

Abort(abort: bool = True) → None[source]

Abort the robot’s motion by setting the abort flag and changing the control mode to -2.

Parameters:: abort (bool, optional) – Whether to abort the motion. Default is True.
Return type:: None

StopMotion() → None[source]

Stop the robot’s motion by executing a trajectory stop command and setting the robot to stop mode.

Return type:: None

WaitUntilStopped(eps: float = 0.001) → None[source]

Wait until the robot’s joint velocities are below a specified threshold.

Parameters:: eps (float, optional) – The velocity threshold to stop waiting. Default is 0.001.
Return type:: None

WaitUntilDone(timeout: float | None = None) → int[source]

Blocks execution until the motion completion or an optional timeout occurs.

Parameters:

dt (float, optional) – Delay between consecutive checks in seconds. Default is 0.01.
timeout (float, optional) – Maximum time to wait in seconds. If None, the function waits indefinitely. Default is None.

Returns:

A result/error code.

Return type:

int

Notes

This method can be reimplemented in robot subclass
This function is blocking and should not be called from a realtime control loop or high-frequency callback.
If motion finishes before timeout (if set), the function exits early.
If motion never completes and no timeout is specified, this method blocks indefinitely.

Wait(wait: float, dt: float | None = None) → None[source]

Wait for a specified duration by updating the robot state and pausing execution.

Parameters:

wait (float) – The duration to wait in seconds.
dt (float, optional) – The time step between state updates. Default is tsamp.

Return type:

None

Restart() → None[source]

Stop and then start the robot to restart its motion.

Return type:: None

SetMotionCheckCallback(fun: Callable[[...], Any]) → None[source]

Set a callback function to be called for motion checks.

Parameters:: fun (Callable[..., Any]) – The callback function to check motion status.
Return type:: None

EnableMotionCheck(check: bool = True) → None[source]

Enable or disable the motion check callback.

Parameters:: check (bool, optional) – Whether to enable motion check. Default is True.
Return type:: None

DisableMotionCheck() → None[source]

Disable the motion check callback.

Return type:: None

SetCaptureCallback(fun: Callable[[...], Any]) → None[source]

Set the callback function for capture events.

Parameters:: fun (Callable[..., Any]) – The callback function to be called when a capture event occurs.
Return type:: None

StartCapture() → None[source]

Start the capture process, ensuring that the update is enabled.

Return type:: None

StopCapture() → None[source]

Stop the capture process.

Return type:: None

SetUserData(data: Any | None) → None[source]

Set the user data to be used for commands.

Parameters:: data (Any, optional) – The user data to be set for commands.
Return type:: None

GetUserdata() → Any | None[source]

Get the user data associated with the command.

Returns:: The current user data set for the command.
Return type:: Any, optional

static MotionResultStr(code: int) → str[source]

Convert motion result status code to a human-readable explanation.

Parameters:: code (int) – Motion result code returned by the controller.
Returns:: Explanation of the motion result (no code name included).
Return type:: str

robotblockset.robots.isrobot(obj: object) → bool[source]

Check if the given object is an instance of the robot class.

Parameters:: obj (object) – The object to be checked.
Returns:: Returns True if the object is an instance of the robot class, otherwise False.
Return type:: bool

robotblockset.robots.manipulability(J: ndarray) → float[source]

Calculate the manipulability of a robot based on its Jacobian matrix.

The manipulability is calculated as the square root of the determinant of the product of the Jacobian matrix and its transpose.

Parameters:: J (JacobianType) – The Jacobian matrix of the robot. It is expected to have shape (m, n), where m is the number of task space dimensions and n is the number of degrees of freedom.
Returns:: The manipulability measure, which is a scalar value representing the robot’s ability to move in all directions within the task space.
Return type:: float

robotblockset.robots.dkin(q: ndarray, kinmodel: Callable[[...], Tuple[ndarray, ndarray]], tcp: ndarray = array([[1.0000, 0.0000, 0.0000, 0.0000], [0.0000, 1.0000, 0.0000, 0.0000], [0.0000, 0.0000, 1.0000, 0.0000], [0.0000, 0.0000, 0.0000, 1.0000]]), out: str = 'x') → ndarray[source]

Direct kinematics.

This function computes the task position for a given joint position using the provided kinematics model.

Parameters:

q (JointConfigurationType) – Joint positions (nj,). An array of joint values.
kinmodel (Callable[..., Tuple[Union[Pose3DType, HomogeneousMatrixType], JacobianType]]) – The direct kinematics function. This function takes joint positions and TCP as input and returns the task position.
tcp (TCPType, optional) – Tool center point pose (7,) or (4, 4). Default is the identity matrix np.eye(4).
out (str, optional) – Specifies the output form. Default is “x” (task positions). Other options can be defined depending on the kinematics model.

Returns:

Task pose in the representation requested by out.

Return type:

Pose3DType or HomogeneousMatrixType

robotblockset.robots.dkinpath(path: ndarray, kinmodel: Callable[[...], Tuple[ndarray, ndarray]], tcp: ndarray = array([[1.0000, 0.0000, 0.0000, 0.0000], [0.0000, 1.0000, 0.0000, 0.0000], [0.0000, 0.0000, 1.0000, 0.0000], [0.0000, 0.0000, 0.0000, 1.0000]]), out: str = 'x') → ndarray[source]

Direct kinematics for a path.

This function computes the task positions for a series of joint positions (path) using the provided kinematics model.

Parameters:

path (JointPathType) – Path in joint space - poses (n, nj). A 2D array of joint positions for multiple configurations.
kinmodel (Callable[..., Tuple[Union[Pose3DType, HomogeneousMatrixType], JacobianType]]) – The direct kinematics function. This function takes joint positions and TCP as input and returns the task position.
tcp (TCPType, optional) – Tool center point pose (7,) or (4, 4). Default is the identity matrix np.eye(4).
out (str, optional) – Specifies the output form. Default is “x” (task positions). Other options can be defined depending on the kinematics model.

Returns:

Task poses corresponding to each joint configuration in the path.

Return type:

Poses3DType or HomogeneousMatrixArrayType

robotblockset.robots.ikin(x: ndarray, q0: ndarray, kinmodel: Callable[[...], Tuple[ndarray, ndarray]], tcp: ndarray = array([[1.0000, 0.0000, 0.0000, 0.0000], [0.0000, 1.0000, 0.0000, 0.0000], [0.0000, 0.0000, 1.0000, 0.0000], [0.0000, 0.0000, 0.0000, 1.0000]]), tsamp: float = 0.01, max_iterations: int = 1000, pos_err: float = 0.0001, ori_err: float = 0.001, task_DOF: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] = array([1, 1, 1, 1, 1, 1]), null_space_task: str = 'None', q_min: ndarray | None = None, q_max: ndarray | None = None, q_opt: ndarray | None = None, v_ns: ndarray | None = None, qdot_ns: ndarray | None = None, x_opt: ndarray | None = None, Kp: float = 10, Kns: float = 1, save_path: bool = False) → Tuple[ndarray, int][source]

Inverse kinematics.

This function computes the joint positions for a given target Cartesian pose using inverse kinematics based on kinematic controller.

Parameters:

x (Pose3DType) – Target Cartesian pose (7,).
q0 (JointConfigurationType) – Initial joint positions (nj,).
kinmodel (Callable[..., Tuple[Union[Pose3DType, HomogeneousMatrixType], JacobianType]]) – Direct kinematics function that computes the task pose given joint positions.
tcp (TCPType, optional) – Tool center point pose. Default is the identity matrix np.eye(4).
tsamp (float, optional) – Sampling time. Default is 0.01.
max_iterations (int, optional) – Maximum number of iterations. Default is 1000.
pos_err (float, optional) – Position error tolerance. Default is 0.0001.
ori_err (float, optional) – Orientation error tolerance. Default is 0.001.
task_DOF (ArrayLike, optional) – Degrees of freedom for the task (6,). Default is all 6 DOFs.
null_space_task (str, optional) – Type of null-space task. Default is “None”. Other options include “Manipulability”, “JointLimits”, etc.
q_min (JointConfigurationType, optional) – Joint limits for the minimum joint positions (nj,). Default is None.
q_max (JointConfigurationType, optional) – Joint limits for the maximum joint positions (nj,). Default is None.
q_opt (JointConfigurationType, optional) – Optimal joint configuration for certain null-space tasks. Default is None.
v_ns (JointConfigurationType, optional) – Null-space task velocity vector (6,). Default is None.
qdot_ns (JointConfigurationType, optional) – Joint velocity for the null-space task (nj,). Default is None.
x_opt (Pose3DType, optional) – Optimal task pose for pose optimization tasks. Default is None.
Kp (float, optional) – Proportional gain for task-space control. Default is 10.
Kns (float, optional) – Proportional gain for null-space task. Default is 1.
save_path (bool, optional) – Flag to save the joint path during the iterations. Default is False.

Returns:

Joint positions and status code, or the full joint path and status code when save_path=True.

Return type:

tuple[JointConfigurationType, int] or tuple[JointPathType, int]

robotblockset.robots.ikinpath(path: ndarray, q0: ndarray, kinmodel: Callable[[...], Tuple[ndarray, ndarray]], tcp: ndarray = array([[1.0000, 0.0000, 0.0000, 0.0000], [0.0000, 1.0000, 0.0000, 0.0000], [0.0000, 0.0000, 1.0000, 0.0000], [0.0000, 0.0000, 0.0000, 1.0000]]), tsamp: float = 0.01, max_iterations: int = 1000, pos_err: float = 0.001, ori_err: float = 0.001, task_DOF: ndarray | List[float] | List[int] | Tuple[float, ...] | Tuple[int, ...] = array([1, 1, 1, 1, 1, 1]), null_space_task: str = 'None', q_min: ndarray | None = None, q_max: ndarray | None = None, q_opt: ndarray | None = None, v_ns: ndarray | None = None, qdot_ns: ndarray | None = None, x_opt: ndarray | None = None, Kp: float = 10, Kns: float = 1) → Tuple[ndarray, int][source]

Inverse kinematics for a path.

This function computes joint positions for a given path of target Cartesian poses using inverse kinematics.

Parameters:

path (Poses3DType) – Path in Cartesian space as pose array.
q0 (JointConfigurationType) – Initial joint positions (nj,).
kinmodel (Callable[..., Tuple[Union[Pose3DType, HomogeneousMatrixType], JacobianType]]) – Direct kinematics function that computes the task pose given joint positions.
tcp (TCPType, optional) – Tool center point pose. Default is the identity matrix np.eye(4).
tsamp (float, optional) – Sampling time. Default is 0.01.
max_iterations (int, optional) – Maximum number of iterations. Default is 1000.
pos_err (float, optional) – Position error tolerance. Default is 0.001.
ori_err (float, optional) – Orientation error tolerance. Default is 0.001.
task_DOF (ArrayLike, optional) – Degrees of freedom for the task (6,). Default is all 6 DOFs.
null_space_task (str, optional) – Type of null-space task. Default is “None”. Other options include “Manipulability”, “JointLimits”, etc.
q_min (JointConfigurationType, optional) – Joint limits for the minimum joint positions (nj,). Default is None.
q_max (JointConfigurationType, optional) – Joint limits for the maximum joint positions (nj,). Default is None.
q_opt (JointConfigurationType, optional) – Optimal joint configuration for certain null-space tasks. Default is None.
v_ns (JointConfigurationType, optional) – Null-space task velocity vector (6,). Default is None.
qdot_ns (JointConfigurationType, optional) – Joint velocity for the null-space task (nj,). Default is None.
x_opt (Pose3DType, optional) – Optimal task pose for pose optimization tasks. Default is None.
Kp (float, optional) – Proportional gain for task-space control. Default is 10.
Kns (float, optional) – Proportional gain for null-space task. Default is 1.

Returns:

Joint positions for each target pose together with the status code.

Return type:

tuple[JointPathType, int]

Functions

`CommandModeStr`(mode)	Convert command mode code to a descriptive human-readable string.
`MotionResultStr`(code)	Convert motion result code to a descriptive human-readable string.
`dkin`(q, kinmodel[, tcp, out])	Direct kinematics.
`dkinpath`(path, kinmodel[, tcp, out])	Direct kinematics for a path.
`ikin`(x, q0, kinmodel[, tcp, tsamp, ...])	Inverse kinematics.
`ikinpath`(path, q0, kinmodel[, tcp, tsamp, ...])	Inverse kinematics for a path.
`isrobot`(obj)	Check if the given object is an instance of the robot class.
`manipulability`(J)	Calculate the manipulability of a robot based on its Jacobian matrix.

Classes

`CommandModeCodes`(*values)	Major command mode codes used by robot motion commands.
`MotionResultCodes`(*values)	Result codes returned by robot motion commands.
`robot`(**kwargs)	Represents a robot master class with various configurations, transformations, and control mechanisms.