Source code for robotblockset.platforms
"""Platform base classes and utilities.
This module defines the common platform abstraction used in robotblockset for
mobile bases and related motion-control components. It provides state and
command containers, default motion parameters, motion result codes, and the
main `platform` base class with support for coordinate transforms, motion
commands, attached robots, and asynchronous execution helpers.
Copyright (c) 2024- Jozef Stefan Institute
Authors: Leon Zlajpah.
"""
from __future__ import annotations
import numpy as np
from typing import Callable, Optional, Any, Tuple, Union
from time import perf_counter, sleep
from threading import Semaphore, Thread
import platform as platform_os
import copy
from enum import Enum
from robotblockset.tools import rbs_object, _eps, rbs_type, check_option, vector, isvector, isscalar, wrap_to_pi
from robotblockset.transformations import map_pose, q2rpy, x2t, t2x, r2q, v2s, xerr, terr, qerr, rot_z, q2r
from robotblockset.trajectories import jtraj
from robotblockset.rbs_typing import ArrayLike, JointConfigurationType, JointVelocityType, JointTorqueType, Pose3DType, Pose3DErrorType, QuaternionType, RotationMatrixType, HomogeneousMatrixType, Velocity3DType, Vector2DType, Vector3DType, WrenchType, JacobianType
from robotblockset.robots import robot, MotionResultCodes, MotionResultStr, CommandModeCodes, CommandModeStr
flag = True
def _dummy() -> None:
"""
Clear the module-level dummy flag.
Returns
-------
None
"""
global flag
flag = False
class _actual:
"""
Represents the actual state of the platform, including planar pose, velocity, and wrench data.
Attributes
----------
q : Optional[JointConfigurationType]
Platform configuration variables.
qdot : Optional[JointVelocityType]
Platform configuration velocities.
trq : Optional[JointTorqueType]
Platform generalized torques or effort values.
x : Optional[Pose3DType]
Actual platform pose.
v : Optional[Velocity3DType]
Actual platform spatial velocity.
FT : Optional[WrenchType]
Actual force/torque data associated with the platform.
"""
def __init__(self) -> None:
"""
Initialize the container for the measured platform state.
Returns
-------
None
This constructor initializes the actual-state container in place.
"""
self.q: Optional[JointConfigurationType] = None
self.qdot: Optional[JointVelocityType] = None
self.trq: Optional[JointTorqueType] = None
self.x: Optional[Pose3DType] = None
self.v: Optional[Velocity3DType] = None
self.FT: Optional[WrenchType] = None
class _command:
"""
Represents the commanded state of the platform, including desired motion, wrench, and control inputs.
Attributes
----------
q : Optional[JointConfigurationType]
Commanded platform configuration variables.
qdot : Optional[JointVelocityType]
Commanded platform configuration velocities.
trq : Optional[JointTorqueType]
Commanded platform generalized torques or effort values.
x : Optional[Pose3DType]
Commanded platform pose.
v : Optional[Velocity3DType]
Commanded platform spatial velocity.
FT : Optional[WrenchType]
Commanded force/torque data.
u : Optional[Velocity3DType]
Control input used by platform controllers.
ux : Optional[Velocity3DType]
Additional control input in task or Cartesian space.
data : Optional[Any]
User-defined command data associated with the platform.
mode : Optional[float]
Command mode identifier.
"""
def __init__(self) -> None:
"""
Initialize the container for commanded platform values.
Returns
-------
None
This constructor initializes the command-state container in place.
"""
self.q: Optional[JointConfigurationType] = None # Commanded joint positions
self.qdot: Optional[JointVelocityType] = None # Commanded joint velocities
self.trq: Optional[JointTorqueType] = None # Commanded joint torques
self.x: Optional[Pose3DType] = None # Commanded Cartesian pose
self.v: Optional[Velocity3DType] = None # Commanded Cartesian velocities
self.FT: Optional[WrenchType] = None # Commanded force/torque data
self.u: Optional[Velocity3DType] = None # Control input
self.ux: Optional[Velocity3DType] = None # Control input for Cartesian space
self.data: Optional[Any] = None # User-defined data
self.mode: Optional[float] = None # Control mode
class _default:
"""
Class to store default parameters for robot behavior.
Attributes
----------
State : str
Default state for robot (Actual/Commanded)
TaskSpace : str
Default task space
TaskPoseForm : str
Default pose form
TaskOriForm : str
Default orientation form
TaskVelForm : str
Default velocity form
TaskFTForm : str
Default force/torque form
Refresh : bool
Whether to refresh the platform state.
Traj : str
Default trajectory type
VelFac : float
Default velocity scaling factor
PosErr : float
Default position error tolerance
OriErr : float
Default orientation error tolerance
LaserAngleRange : float
Default laser angle range
CheckObstacles : bool
Flag to check obstacles
ObstacleMaxDist : float
Maximum distance for obstacles
ObstacleMinDist : float
Minimum distance for obstacles
ObstaclesForPlatform : bool
Flag to include obstacles for platform
MinVel: float
Minimal velocity (used in stop platform when close to obstacle)
ApproachDist : float
Approach distance
Krot : float
Rotation motion controller constant
Kdist : float
Linear motion controller constant
Kdir : float
Direction motion controller constant
Wait : float
Default wait time
UpdateTime : float
Update time interval
"""
def __init__(self) -> None:
"""
Initialize the default platform configuration parameters.
Returns
-------
None
This constructor initializes the default-parameter container in place.
"""
self.State: str = "Actual" # Default state for robot (Actual/Commanded)
self.TaskSpace: str = "World" # Default task space
self.TaskPoseForm: str = "2d" # Default pose form
self.TaskOriForm: str = "Theta" # Default orientation form
self.TaskVelForm: str = "Twist" # Default velocity form
self.TaskFTForm: str = "Wrench" # Default force/torque form
self.Refresh: bool = True # Whether to refresh the platform state
self.Traj: str = "Poly" # Default trajectory type
self.VelFac: float = 0.25 # Default velocity scaling factor
self.PosErr: float = 0.01 # Default position error tolerance
self.OriErr: float = 0.01 # Default orientation error tolerance
self.LaserAngleRange: float = np.pi / 4 # Default laser angle range
self.CheckObstacles: bool = True # Flag to check obstacles
self.ObstacleMaxDist: float = 1 # Maximum distance for obstacles
self.ObstacleMinDist: float = 0.4 # Minimum distance for obstacles
self.ObstaclesForPlatform: bool = True # Flag to include obstacles for platform
self.MinVel: float = 0.001 # Minimum velocity
self.ApproachDist: float = 0.5 # Approach distance
self.Krot: float = 10 # Rotation motion controller constant
self.Kdist: float = 4 # Linear motion controller constant
self.Kdir: float = 20 # Direction motion controller constant
self.Wait: float = 0.1 # Default wait time
self.UpdateTime: float = 1.0 # Update time interval
[docs]
class platform(rbs_object):
"""
Represents a mobile platform base class with state handling, frame transforms, and motion-control utilities.
Attributes
----------
Name : str
Name of the platform.
tsamp : float
Sampling rate for the platform.
TRobotBase : HomogeneousMatrixType
Transformation matrix of the attached robot base relative to the platform.
TObject : HomogeneousMatrixType
Transformation matrix of the tracked or manipulated object.
Robot : Optional[robot]
Robot attached to the platform, if any.
User : Optional[Any]
User-defined data or object associated with the platform.
Tag : Optional[str]
Tag associated with the platform.
"""
[docs]
def __init__(self, **kwargs: Any) -> None:
"""
Initialize the platform with default values and optional configuration arguments.
Parameters
----------
**kwargs : Any
Optional arguments for custom configuration or parameters.
Returns
-------
None
"""
rbs_object.__init__(self)
self.Name: str = "Platform" # Platform name
self.tsamp: float = 0.01 # Sampling rate
self.TRobotBase: HomogeneousMatrixType = np.eye(4) # Robot base transformation matrix
self.TObject: HomogeneousMatrixType = np.eye(4) # Object transformation matrix
self.Robot: Optional[robot] = None # Robot attached to platform
self.User: Optional[Any] = None # User data or object
self.Tag: Optional[str] = None # Tag for platform
self._t0: float = 0 # Initial time
self._tt: float = 0 # Actual robot time
self._tt0: float = 0 # Initial robot time
self._robottime: float = 0 # Time from simulator
self._last_update: float = -100 # Last update time
self._last_control_time: float = -100 # Last control time
self._command: _command = _command() # Commanded values
self._actual: _actual = _actual() # Measured values
self._default: _default = _default() # Default options
self._do_update: bool = True # Flag to enable state update
self._do_capture: bool = False # Flag to enable callback capture
self._capture_callback: Optional[Any] = None # Callback function in Update
self._do_motion_check: bool = False # Flag to enable motion checks
self._motion_check_callback: Optional[Any] = None # Callback during motion
self._motion_error: Optional[Any] = None # Motion controller error status (can used to stop motion)
self._last_status: int = 0 # Last motion command status
self._platform_autonomous_motion: Optional[Any] = None # Autonomous motion callback
self._abort_autonomous_motion: bool = False # Flag to abort autonomous motion
self._control_strategy: str = "CartesianVelocity" # Control strategy
self._semaphore: Semaphore = Semaphore(1) # Semaphore for asynchronous motion
self._threads_active: bool = platform_os.system() == "Linux" # Flag for threads on Linux
self._abort_motion: bool = False # Flag to abort current motion
self._connected: bool = False # Connection status
self._verbose: int = 1 # Verbosity level
[docs]
def reset_threads(self) -> None:
"""
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.
Returns
-------
None
"""
self._semaphore = Semaphore(1)
[docs]
def jointvar(self, x: ArrayLike) -> JointConfigurationType:
"""
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
-------
JointConfigurationType
The input array `x` if it has the correct shape.
Raises
------
TypeError
If the input `x` does not have the proper shape.
Notes
-----
Assuming 'nj' represents the number of joints, defined elsewhere in the class
"""
x = np.asarray(x)
if x.shape[-1] == self.nj:
return x
else:
raise TypeError("Parameter has not proper shape")
[docs]
def spatial(self, x: ArrayLike) -> np.ndarray:
"""
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
-------
np.ndarray
The input `x` in the validated shape, possibly modified if the shape was (3, 4).
Raises
------
TypeError
If the input `x` does not have a valid shape.
"""
x = rbs_type(x)
# Check for valid shapes
if x.shape == (7,) or x.shape == (4, 4) or x.shape == (3,) or x.shape == (4,) or x.shape == (3, 3) or x.shape == (6,):
return x
elif x.shape == (3, 4):
x = np.vstack((x, np.array([0, 0, 0, 1])))
return x
else:
raise TypeError("Parameter has not proper shape")
[docs]
def simtime(self) -> float:
"""
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
-------
float
The current simulation time in seconds since an arbitrary point (usually the start of the program).
"""
return perf_counter()
def _sleep(self, time: float) -> None:
"""
Pause execution for the given duration.
Parameters
----------
time : float
Time in seconds to sleep.
Returns
-------
None
"""
sleep(time)
def _synchro_control(self, wait: float) -> None:
"""
Synchronizes the control loop by waiting for the specified time interval to elapse.
This method calculates the time difference since the last control update, and if the elapsed
time is less than the specified wait time, it waits for the remaining time. If the system
supports threading, it sleeps for half of the remaining wait time to avoid blocking the control loop.
Parameters
----------
wait : float
The amount of time (in seconds) to wait before proceeding with the next control update.
Returns
-------
None
This method does not return any value, it only modifies the internal state of the robot.
"""
remaining = self._last_control_time + wait - self.simtime()
if remaining > 0:
self._sleep(remaining)
self._last_control_time = self.simtime()
[docs]
def UseThreads(self, active: bool) -> None:
"""
Enable or disable worker threads for platform control.
This method sets the `_threads_active` attribute, which controls whether the platform
control system should use threads for asynchronous operations.
Parameters
----------
active : bool
A boolean value that indicates whether threads should be active.
If `True`, threads are enabled. If `False`, threads are disabled.
Returns
-------
None
This method does not return any value.
"""
self._threads_active = active
[docs]
def SetTsamp(self, tsamp: float) -> None:
"""
Set the sampling time for the platform control system.
This method updates the `tsamp` attribute and synchronizes the default wait interval.
Parameters
----------
tsamp : float
The new sampling time in seconds to set for the robot control system.
Returns
-------
None
This method does not return any value.
"""
if tsamp <= 0:
raise ValueError("Sample time must be positive")
self.tsamp = tsamp
self._default.Wait = tsamp
[docs]
def ResetTime(self) -> None:
"""
Reset the platform timing reference to the current simulation time.
This method updates the internal time attributes `_t0` and `_tt0` to the current
simulation time and current platform time, respectively.
Returns
-------
None
This method does not return any value.
"""
self.GetState()
self._t0 = self.simtime()
self._tt0 = copy.deepcopy(self._tt)
self.Update()
[docs]
def isConnected(self) -> bool:
"""
Checks if the platform is connected.
Returns
-------
bool
True if the platform is connected, False otherwise.
"""
return self._connected
[docs]
def isReady(self) -> bool:
"""
Check if the platform is ready for operation.
This method checks the `_connected` attribute to determine if the platform is connected
and operational.
Returns
-------
bool
`True` if the robot is connected and ready for operations, otherwise `False`.
"""
return self._connected
[docs]
def isActive(self) -> bool:
"""
Check if the platform target is active.
This method always returns `True`, indicating that the platform target is in an active state.
It can be overridden in subclasses for more complex behavior.
Returns
-------
bool
Always returns `True`, indicating the platform target is active.
"""
return True
[docs]
def inMotion(self) -> bool:
"""
Check if the platform is in motion.
Returns
-------
bool
`True` indicating the platform is excetuting motion command.
"""
return self._command.mode > CommandModeCodes.STOP.value
[docs]
def Check(self, silent: bool = False) -> list[str]:
"""
Check the status of the platform.
Parameters
----------
silent : bool, optional
If `True`, suppress status messages while checking the platform state.
Returns
-------
list[str]
A list containing non-active status entries and descriptions.
"""
return []
[docs]
def HasError(self) -> bool:
"""
Check whether the platform reports any errors.
Returns
-------
bool
``True`` if :meth:`Check` returns at least one error entry,
otherwise ``False``.
"""
return len(self.Check(silent=True)) > 0
@property
def Time(self) -> float:
"""
Get the elapsed wall time since the platform was initialized.
Returns
-------
float
Elapsed time in seconds.
"""
return self.simtime() - self._t0
@property
def t(self) -> float:
"""
Get the elapsed platform time.
Returns
-------
float
Time difference in seconds.
"""
return self._tt - self._tt0
@property
def command(self) -> _command:
"""
Get the commanded state of the platform.
Returns
-------
_command
A copy of the commanded state.
"""
return copy.deepcopy(self._command)
@property
def actual(self) -> _actual:
"""
Get the actual state of the platform.
Returns
-------
_actual
A copy of the actual state.
"""
return copy.deepcopy(self._actual)
@property
def q(self) -> JointConfigurationType:
"""
Get the current joint positions.
Returns
-------
np.ndarray
Joint positions (nj,).
"""
return copy.deepcopy(self._actual.q)
@property
def qdot(self) -> JointVelocityType:
"""
Get the current joint velocities.
Returns
-------
np.ndarray
Joint velocities (nj,).
"""
return copy.deepcopy(self._actual.qdot)
@property
def trq(self) -> JointTorqueType:
"""
Get the current joint torques.
Returns
-------
np.ndarray
Joint torques (nj,).
"""
return copy.deepcopy(self._actual.trq)
@property
def x(self) -> Pose3DType:
"""
Get the current platform pose.
Returns
-------
np.ndarray
Platform pose (7,).
"""
return copy.deepcopy(self._actual.x)
@property
def p(self) -> Vector2DType:
"""
Get the current planar platform position.
Returns
-------
np.ndarray
Platform position (2,).
"""
return copy.deepcopy(self._actual.x[:2])
@property
def Q(self) -> QuaternionType:
"""
Get the current platform orientation as a quaternion.
Returns
-------
np.ndarray
Platform quaternion (4,).
"""
return copy.deepcopy(self.GetPose(state="Actual", task_space="World", out="Q"))
@property
def R(self) -> RotationMatrixType:
"""
Get the current platform orientation as a rotation matrix.
Returns
-------
np.ndarray
Platform rotation matrix (3, 3).
"""
return copy.deepcopy(self.GetPose(state="Actual", task_space="World", out="R"))
@property
def T(self) -> HomogeneousMatrixType:
"""
Get the current platform pose as a homogeneous transformation matrix.
Returns
-------
np.ndarray
Platform transformation matrix (4, 4).
"""
return copy.deepcopy(self.GetPose(state="Actual", task_space="World", out="T"))
@property
def theta(self) -> float:
"""
Get the current platform yaw angle.
Returns
-------
float
The rotation about the world z-axis extracted from the platform orientation.
"""
return q2rpy(copy.deepcopy(self._actual.x[3:]))[0]
@property
def v(self) -> Velocity3DType:
"""
Get the current platform spatial velocity.
Returns
-------
np.ndarray
Platform velocity (6,).
"""
return copy.deepcopy(self.GetVel(state="Actual", task_space="World", out="Twist"))
@property
def pdot(self) -> Vector2DType:
"""
Get the current planar platform linear velocity.
Returns
-------
np.ndarray
Platform linear velocity (2,).
"""
return copy.deepcopy(self._actual.v[:2])
@property
def w(self) -> Vector3DType:
"""
Get the current platform angular velocity.
Returns
-------
np.ndarray
Platform angular velocity (3,).
"""
return copy.deepcopy(self._actual.v[3:])
@property
def FT(self) -> WrenchType:
"""
Get the current force/torque sensor data.
Returns
-------
np.ndarray
Force/Torque sensor data (6,).
"""
return copy.deepcopy(self.GetFT(state="Actual", task_space="World", out="Wrench"))
@property
def F(self) -> Vector3DType:
"""
Get the current force sensor data.
Returns
-------
np.ndarray
Force sensor data (3,) or (..., 3).
"""
return copy.deepcopy(self.GetFT(state="Actual", task_space="World", out="Force"))
@property
def Trq(self) -> Vector3DType:
"""
Get the current torque sensor data.
Returns
-------
np.ndarray
Torque sensor data (3,) or (..., 3).
"""
return copy.deepcopy(self.GetFT(state="Actual", task_space="World", out="Torque"))
@property
def q_ref(self) -> JointConfigurationType:
"""
Get the commanded platform configuration.
Returns
-------
np.ndarray
Desired joint positions (nj,).
"""
return copy.deepcopy(self._command.q)
@property
def qdot_ref(self) -> JointVelocityType:
"""
Get the commanded platform configuration velocities.
Returns
-------
np.ndarray
Desired joint velocities (nj,).
"""
return copy.deepcopy(self._command.qdot)
@property
def x_ref(self) -> Pose3DType:
"""
Get the commanded platform pose.
Returns
-------
np.ndarray
Desired platform pose (7,).
"""
return copy.deepcopy(self._command.x)
@property
def p_ref(self) -> Vector2DType:
"""
Get the commanded planar platform position.
Returns
-------
np.ndarray
Desired platform position (2,).
"""
return copy.deepcopy(self._command.x[:2])
@property
def Q_ref(self) -> QuaternionType:
"""
Get the commanded platform orientation as a quaternion.
Returns
-------
np.ndarray
Desired platform quaternion (4,).
"""
return copy.deepcopy(self.GetPose(state="Command", task_space="World", out="Q"))
@property
def R_ref(self) -> RotationMatrixType:
"""
Get the commanded platform orientation as a rotation matrix.
Returns
-------
np.ndarray
Desired platform rotation matrix (3, 3).
"""
return copy.deepcopy(self.GetPose(state="Command", task_space="World", out="R"))
@property
def T_ref(self) -> HomogeneousMatrixType:
"""
Get the commanded platform pose as a homogeneous transformation matrix.
Returns
-------
np.ndarray
Desired platform transformation matrix (4, 4).
"""
return copy.deepcopy(self.GetPose(state="Command", task_space="World", out="T"))
@property
def theta_ref(self) -> float:
"""
Get the commanded platform yaw angle.
Returns
-------
float
The rotation about the world z-axis extracted from the commanded orientation.
"""
return q2rpy(copy.deepcopy(self._command.x[3:]))[0]
@property
def v_ref(self) -> Velocity3DType:
"""
Get the commanded platform spatial velocity.
Returns
-------
np.ndarray
Commanded platform velocity (6,).
"""
return copy.deepcopy(self.GetVel(state="Commanded", task_space="World", out="Twist"))
@property
def pdot_ref(self) -> Vector2DType:
"""
Get the commanded planar platform linear velocity.
Returns
-------
np.ndarray
Commanded platform linear velocity (2,).
"""
return copy.deepcopy(self._command.v[:2])
@property
def w_ref(self) -> Vector3DType:
"""
Get the commanded platform angular velocity.
Returns
-------
np.ndarray
Desired platform angular velocity (3,) or (..., 3).
"""
return copy.deepcopy(self._command.v[3:])
@property
def FT_ref(self) -> WrenchType:
"""
Get the desired force/torque sensor data.
Returns
-------
np.ndarray
Desired force/torque sensor data (6,).
"""
return copy.deepcopy(self.GetFT(state="Command", task_space="World", out="Wrench"))
@property
def F_ref(self) -> Vector3DType:
"""
Get the desired force sensor data.
Returns
-------
np.ndarray
Desired force sensor data (3,).
"""
return copy.deepcopy(self.GetFT(state="Command", task_space="World", out="Force"))
@property
def Trq_ref(self) -> Vector3DType:
"""
Get the desired torque sensor data.
Returns
-------
np.ndarray
Desired torque sensor data (3,).
"""
return copy.deepcopy(self.GetFT(state="Command", task_space="World", out="Torque"))
@property
def q_err(self) -> JointConfigurationType:
"""
Get the error in joint positions.
Returns
-------
np.ndarray
Error in joint positions (nj,).
"""
return self.q_ref - self.q
@property
def qdot_err(self) -> JointVelocityType:
"""
Get the error in joint velocities.
Returns
-------
np.ndarray
Error in joint velocities (nj,).
"""
return self.qdot_ref - self.qdot
@property
def x_err(self) -> Pose3DErrorType:
"""
Get the platform pose error.
Returns
-------
np.ndarray
Platform pose error (6,).
"""
return xerr(self.x_ref, self.x)
@property
def p_err(self) -> Vector2DType:
"""
Get the platform position error.
Returns
-------
np.ndarray
Platform position error (2,).
"""
return self.p_ref - self.p
@property
def Q_err(self) -> Vector3DType:
"""
Get the platform quaternion error.
Returns
-------
np.ndarray
Platform quaternion error (3,).
"""
return qerr(self.Q_ref, self.Q)
@property
def R_err(self) -> RotationMatrixType:
"""
Get the platform rotation matrix error.
Returns
-------
np.ndarray
Platform rotation matrix error (3, 3).
"""
return self.R_ref @ self.R.T
@property
def T_err(self) -> Pose3DErrorType:
"""
Get the platform transformation matrix error.
Returns
-------
np.ndarray
Platform transformation matrix error (6, ).
"""
return terr(self.T_ref, self.T)
@property
def theta_err(self) -> float:
"""
Property to calculate the difference between the reference orientation and the current orientation along z-axis.
Returns
-------
float
The difference along z-axis between the reference and current orientation.
"""
return self.theta_ref - self.theta
@property
def v_err(self) -> Velocity3DType:
"""
Get the platform velocity error.
Returns
-------
np.ndarray
Platform velocity error (6,).
"""
return self.v_ref - self.v
@property
def pdot_err(self) -> Vector2DType:
"""
Get the platform linear velocity error.
Returns
-------
np.ndarray
Platform linear velocity error (2,).
"""
return self.pdot_ref - self.pdot
@property
def w_err(self) -> Vector3DType:
"""
Get the platform angular velocity error.
Returns
-------
np.ndarray
Platform angular velocity error (3,).
"""
return self.w_ref - self.w
# Initialization and update
[docs]
def InitObject(self) -> None:
"""
Initialize the platform command and actual state with zeros.
This method sets the initial values for joint positions, joint
velocities, joint torques, platform pose, velocities, force/torque
sensor data, control inputs, and other state variables.
Returns
-------
None
This method modifies the internal state of the platform object.
"""
self._command.q = np.zeros(self.nj)
self._command.qdot = np.zeros(self.nj)
self._command.trq = np.zeros(self.nj)
self._command.u = np.zeros(self.nj)
self._command.x = np.array([0, 0, 0, 1, 0, 0, 0]) # Default pose (identity quaternion)
self._command.rx = np.array([0, 0, 0, 1, 0, 0, 0]) # Default pose (identity quaternion)
self._command.v = np.zeros(6)
self._command.FT = np.zeros(6)
self._command.ux = np.zeros(2)
self._command.data = None
self._command.mode = CommandModeCodes.STOP.value
self._actual.q = np.zeros(self.nj)
self._actual.qdot = np.zeros(self.nj)
self._actual.trq = np.zeros(self.nj)
self._actual.x = np.array([0, 0, 0, 1, 0, 0, 0]) # Default pose (identity quaternion)
self._actual.v = np.zeros(6)
self._actual.FT = np.zeros(6)
self.Js = self.Kinmodel()[-1][[0, 5], :]
self.pJs = np.linalg.pinv(self.Js)
[docs]
def Init(self) -> None:
"""
Initialize the platform state, current target, timing, and log message.
This method calls the following functions to initialize the platform:
1. `InitObject()` - Initializes the command and actual states with default values.
2. `GetState()` - Retrieves the current state of the platform.
3. `ResetCurrentTarget()` - Resets the current target position.
4. `ResetTime()` - Resets the platform internal time.
Returns
-------
None
This method modifies the platform internal state and logs a message.
"""
self.InitObject()
self.GetState()
self.ResetCurrentTarget()
self.ResetTime()
self.DebugMessage("Initialized")
[docs]
def GetState(self) -> None:
"""
Update the platform state.
It has to be reimplemented in a concrete platform class.
It has to set:
- Actual platform joint and task-space states
- Platform sensor readings
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
-------
None
This method modifies the internal state of the platform.
"""
self._tt = self.simtime()
self._last_update = self.simtime()
self.WarningMessage("Not implemented!")
[docs]
def Update(self) -> None:
"""
Updates the platform'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
platform's internal state.
- If ``_do_capture`` is ``True`` and a capture callback function
(``_capture_callback``) is defined, it calls the callback function,
passing the platform object as an argument.
Returns
-------
None
This method does not return any value. It modifies the internal state of the platform and may
trigger a callback.
"""
if self._do_update:
self.GetState()
if self._do_capture and self._capture_callback is not None:
self._capture_callback(self)
elif (self.simtime() - self._last_update) >= self.tsamp:
self.GetState()
[docs]
def EnableUpdate(self) -> None:
"""
Enables the update of the platform's internal state.
This method sets the `_do_update` attribute to `True`, which allows the platform's state to be updated.
Returns
-------
None
This method does not return any value. It modifies the internal state of the platform.
"""
self._do_update = True
[docs]
def DisableUpdate(self) -> None:
"""
Disables the update of the platform's internal state.
This method sets the `_do_update` attribute to `False`, which prevents the platform's state from being updated.
Returns
-------
None
This method does not return any value. It modifies the internal state of the platform.
"""
self._do_update = False
[docs]
def GetUpdateStatus(self) -> bool:
"""
Returns the current status of the update flag.
This method returns the value of the `_do_update` attribute, which indicates whether the platform's state update is enabled.
Returns
-------
bool
`True` if updates are enabled, `False` otherwise.
"""
return self._do_update
[docs]
def ResetCurrentTarget(self) -> None:
"""
Resets the current target to the actual values of joint positions, velocities, torques, and other state variables.
Returns
-------
None
"""
self.GetState()
self._command.q = copy.deepcopy(self._actual.q)
self._command.qdot = np.zeros(self.nj)
self._command.trq = np.zeros(self.nj)
self._command.x = copy.deepcopy(self._actual.x)
self._command.v = np.zeros(6)
self._command.FT = np.zeros(6)
self._command.trq = np.zeros(self.nj)
self._last_control_time = self.simtime()
if self.Robot is not None:
self.Robot.ResetCurrentTarget()
self._sleep(0.1)
self.Update()
# Get joint variables
[docs]
def GetJointPos(self, state: Optional[str] = None, refresh: Optional[bool] = None) -> JointConfigurationType:
"""
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 platform class is used.
refresh : bool, optional
If ``True``, update the platform state before retrieving joint
positions.
Returns
-------
np.ndarray
The joint positions (`q`) from the specified state, copied to prevent external modifications.
Raises
------
ValueError
If the `state` is not "Actual" or "Commanded".
"""
if state is None:
state = self._default.State
if refresh is None:
refresh = self._default.Refresh
if check_option(state, "Actual"):
if refresh:
self.GetState()
return copy.deepcopy(self._actual.q)
elif check_option(state, "Commanded"):
return copy.deepcopy(self._command.q)
else:
raise ValueError(f"State '{state}' not supported")
[docs]
def GetJointVel(self, state: Optional[str] = None, refresh: Optional[bool] = None) -> JointVelocityType:
"""
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 platform class is used.
refresh : bool, optional
If ``True``, update the platform state before retrieving joint
velocities.
Returns
-------
np.ndarray
The joint velocities (`qdot`) from the specified state, copied to prevent external modifications.
Raises
------
ValueError
If the `state` is not "Actual" or "Commanded".
"""
if state is None:
state = self._default.State
if refresh is None:
refresh = self._default.Refresh
if check_option(state, "Actual"):
if refresh:
self.GetState()
return copy.deepcopy(self._actual.qdot)
elif check_option(state, "Commanded"):
return copy.deepcopy(self._command.qdot)
else:
raise ValueError(f"State '{state}' not supported")
[docs]
def GetJointTrq(self, state: Optional[str] = None, refresh: Optional[bool] = None) -> JointTorqueType:
"""
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 platform class is used.
refresh : bool, optional
If ``True``, update the platform state before retrieving joint
torques.
Returns
-------
np.ndarray
The joint torques (`trq`) from the specified state, copied to prevent external modifications.
Raises
------
ValueError
If the `state` is not "Actual" or "Commanded".
"""
if state is None:
state = self._default.State
if refresh is None:
refresh = self._default.Refresh
if check_option(state, "Actual"):
if refresh:
self.GetState()
return copy.deepcopy(self._actual.trq)
elif check_option(state, "Commanded"):
return copy.deepcopy(self._command.trq)
else:
raise ValueError(f"State '{state}' not supported")
# Get task space variables
[docs]
def GetPose(self, out: Optional[str] = None, task_space: Optional[str] = None, state: Optional[str] = None, refresh: Optional[bool] = None) -> Pose3DType:
"""Get platform pose
Parameters
----------
out : str, optional
Output form, by default "x" ("Pose")
task_space : str, optional
Task space frame, by default "World"
state : str, optional
Variable state, by default "Actual"
refresh : bool, optional
If ``True``, update the platform state before retrieving the pose.
Returns
-------
array of floats
Platform pose
"""
if out is None:
out = self._default.TaskPoseForm
if state is None:
state = self._default.State
if task_space is None:
task_space = self._default.TaskSpace
if refresh is None:
refresh = self._default.Refresh
if refresh:
self.GetState()
elif (self.simtime() - self._last_update) > (self.tsamp * 0.9):
self.GetState()
if check_option(state, "Actual"):
_x = copy.deepcopy(self._actual.x)
elif check_option(state, "Commanded"):
_x = copy.deepcopy(self._command.x)
else:
raise ValueError(f"State {state} not supported in GetPose")
if check_option(task_space, "World"):
pass
elif check_option(task_space, "Object"):
_x = self.WorldToObject(_x)
elif check_option(task_space, "Platform"):
_x = self.WorldToPlatform(_x)
else:
raise ValueError(f"Task space {task_space} not supported in GetPose")
return map_pose(x=_x, out=out)
[docs]
def GetPos(self, out: Optional[str] = None, task_space: Optional[str] = None, state: Optional[str] = None) -> Vector3DType:
"""Get platform position
Parameters
----------
out : str, optional
Output form, by default "p" ("Position")
task_space : str, optional
Task space frame, by default "World"
state : str, optional
Variable state, by default "Actual"
Returns
-------
array of floats
platform position (3,)
"""
if out is None:
out = self._default.TaskPoseForm
if check_option(out, "2d"):
out = "XY"
if out in ["Position", "p", "XY"]:
return self.GetPose(out=out, task_space=task_space, state=state)
else:
raise ValueError(f"Output form {out} not supported in GetPos")
[docs]
def GetOri(self, out: Optional[str] = None, task_space: Optional[str] = None, state: Optional[str] = None) -> Union[QuaternionType, RotationMatrixType]:
"""Get platform orientation
Parameters
----------
out : str, optional
Output form, by default "Q" ("Quaternion")
task_space : str, optional
Task space frame, by default "World"
state : str, optional
Variable state, by default "Actual"
Returns
-------
array of floats
platform orientation (4,) or (3,3)
"""
if out is None:
out = self._default.TaskOriForm
if out in ["2d", "Theta"]:
_x = self.GetPose(out="Q", task_space=task_space, state=state)
return q2rpy(_x)[0]
elif out in ["Quaternion", "Q", "RotationMatrix", "R", "Angle"]:
return self.GetPose(out=out, task_space=task_space, state=state)
else:
raise ValueError(f"Output form {out} not supported in GetOri")
[docs]
def GetVel(self, out: Optional[str] = None, task_space: Optional[str] = None, state: Optional[str] = None, refresh: Optional[bool] = None) -> Velocity3DType:
"""Get platform velocity
Parameters
----------
out : str, optional
Output form, by default "Twist"
task_space : str, optional
Task space frame, by default "World"
state : str, optional
Variable state, by default "Actual"
refresh : bool, optional
If ``True``, update the platform state before retrieving the velocity.
Returns
-------
array of floats
Platform velocity (6,) or (3,)
"""
if out is None:
out = self._default.TaskVelForm
if state is None:
state = self._default.State
if task_space is None:
task_space = self._default.TaskSpace
if refresh is None:
refresh = self._default.Refresh
if refresh:
self.GetState()
elif (self.simtime() - self._last_update) > (self.tsamp * 0.9):
self.GetState()
if check_option(state, "Actual"):
_vv = copy.deepcopy(self._actual.v)
elif check_option(state, "Commanded"):
_vv = copy.deepcopy(self._command.v)
else:
raise ValueError(f"State {state} not supported")
if check_option(task_space, "World"):
pass
elif check_option(task_space, "Object"):
_vv = self.WorldToObject(_vv)
elif check_option(task_space, "Platform"):
_vv = self.WorldToPlatform(_vv)
else:
raise ValueError(f"Task space {task_space} not supported in GetVel")
if check_option(out, "Twist"):
return _vv
elif check_option(out, "Linear"):
return _vv[:3]
elif check_option(out, "Angular"):
return _vv[3:]
elif check_option(out, "2d"):
return _vv[[0, 1, 5]]
else:
raise ValueError(f"Output form {out} not supported")
[docs]
def GetFT(self, out: Optional[str] = None, task_space: Optional[str] = None, state: Optional[str] = None, avg_time: float = 0, refresh: Optional[bool] = None) -> WrenchType:
"""
Get the platform force/torque data.
Parameters
----------
out : str, optional
Output form for the wrench. Supported values are ``"Wrench"``,
``"Force"``, ``"Torque"``, and ``"2d"``.
task_space : str, optional
Task space frame to use. Supported values are ``"World"``,
``"Object"``, and ``"Platform"``.
state : str, optional
Platform state to use. Supported values are ``"Actual"`` and
``"Commanded"``.
avg_time : float, optional
Averaging time in seconds. This base implementation accepts the
argument for API compatibility but does not apply averaging.
refresh : bool, optional
If ``True``, update the platform state before retrieving the data.
Returns
-------
WrenchType
Force/torque data in the requested output form.
Raises
------
ValueError
If ``state``, ``task_space``, or ``out`` is not supported.
"""
if out is None:
out = self._default.TaskFTForm
if state is None:
state = self._default.State
if task_space is None:
task_space = self._default.TaskSpace
if refresh is None:
refresh = self._default.Refresh
if refresh:
self.GetState()
elif (self.simtime() - self._last_update) > (self.tsamp * 0.9):
self.GetState()
if check_option(state, "Actual"):
_FT = self._actual.FT # in EE (tool) CS
elif check_option(state, "Commanded"):
_FT = self._command.FT # in robot CS
else:
raise ValueError(f"State {state} not supported")
if check_option(task_space, "World"):
pass
elif check_option(task_space, "Object"):
_FT = self.WorldToObject(_FT, typ="Wrench")
elif check_option(task_space, "Platform"):
_FT = self.WorldToPlatform(_FT, typ="Wrench")
else:
raise ValueError(f"Task space {state} not supported in GetFT")
if check_option(out, "Wrench"):
return _FT
elif check_option(out, "Force"):
return _FT[:3]
elif check_option(out, "Torque"):
return _FT[3:]
elif check_option(out, "2d"):
return _FT[[0, 1, 5]]
else:
raise ValueError(f"Output form {out} not supported")
# Task space motion
[docs]
def Set_vel(self, v: ArrayLike, wait: Optional[float] = None) -> int:
"""
Command a planar platform velocity for one control interval.
Parameters
----------
v : ArrayLike
Planar velocity command ``[linear_velocity, angular_velocity]``.
wait : float, optional
Time in seconds to hold the command. If ``None``, :attr:`tsamp` is
used.
Returns
-------
int
Motion result code. The base implementation returns
:attr:`MotionResultCodes.MOTION_SUCCESS.value`.
"""
if wait is None:
wait = self.tsamp
v = vector(v, dim=2)
self._command.u = self.pJs @ v
self._command.ux = v
self._command.v = np.array([v[0], 0.0, 0.0, 0.0, 0.0, v[1]])
self._sleep(wait)
return MotionResultCodes.MOTION_SUCCESS.value
[docs]
def CMoveToOri(self, rtheta: float, task_space: Optional[str] = None, wait: Optional[float] = None, vel_fac: Optional[float] = None, ori_err: Optional[float] = None, k_rot: Optional[float] = None, asynchronous: bool = False) -> Union[Thread, int]:
"""
Move the platform to a target planar orientation.
Parameters
----------
rtheta : float
Target yaw angle in radians.
task_space : str, optional
Frame in which ``rtheta`` is expressed. Supported values are
``"World"``, ``"Object"``, and ``"Platform"``. If ``None``, the
default task space is used.
wait : float, optional
Maximum motion duration in seconds. If ``None``, the default wait
value is used and may be expanded based on the target distance.
vel_fac : float, optional
Velocity scaling factor. If ``None``, the default value is used.
ori_err : float, optional
Orientation tolerance in radians. If ``None``, the default value is
used.
k_rot : float, optional
Proportional orientation-control gain. If ``None``, the default
value is used.
asynchronous : bool, optional
If ``True``, execute the motion in a daemon thread and return that
thread.
Returns
-------
Thread or int
The worker thread for asynchronous execution, otherwise a motion
result code.
"""
if asynchronous:
self.DebugMessage("ASYNC CMoveToOri")
_th = Thread(target=self._CMoveToOri, args=(rtheta,), kwargs={"task_space": task_space, "wait": wait, "vel_fac": vel_fac, "ori_err": ori_err, "k_rot": k_rot}, daemon=True)
_th.start()
return _th
else:
return self._CMoveToOri(rtheta, task_space=task_space, wait=wait, vel_fac=vel_fac, ori_err=ori_err, k_rot=k_rot)
def _CMoveToOri(self, rtheta: float, task_space: Optional[str] = None, wait: Optional[float] = None, vel_fac: Optional[float] = None, ori_err: Optional[float] = None, k_rot: Optional[float] = None) -> int:
"""
Execute a blocking planar orientation move.
Parameters
----------
rtheta : float
Target yaw angle in radians.
task_space : str, optional
Frame in which ``rtheta`` is expressed. Supported values are
``"World"``, ``"Object"``, and ``"Platform"``.
wait : float, optional
Maximum motion duration in seconds.
vel_fac : float, optional
Velocity scaling factor.
ori_err : float, optional
Orientation tolerance in radians.
k_rot : float, optional
Proportional orientation-control gain.
Returns
-------
int
Motion result code.
Raises
------
ValueError
If ``task_space`` is not supported.
"""
if task_space is None:
task_space = self._default.TaskSpace
if wait is None:
wait = self._default.Wait
if vel_fac is None:
vel_fac = self._default.VelFac
if ori_err is None:
ori_err = self._default.OriErr
if k_rot is None:
k_rot = self._default.Krot
if check_option(task_space, "World"):
pass
elif check_option(task_space, "Object"):
rtheta = self.ObjectToWorld(rtheta)
elif check_option(task_space, "Platform"):
rtheta = self.PlatformToWorld(rtheta)
else:
raise ValueError(f"Task space {task_space} not supported in CMoveToOri")
if wait == self._default.Wait:
_eo = rtheta - self.theta
wait = max(_eo / self.v_max[1], _eo / self.v_min[1]) * 10
if not self._semaphore.acquire(blocking=False):
self.WarningMessage("CMoveToOri not executed due to active threads!")
return MotionResultCodes.ACTIVE_THREADS.value
self.DebugMessage(f"CMoveToOri started: theta={rtheta:.3f}")
if not self.Start():
self._semaphore.release()
return MotionResultCodes.NOT_READY.value
self._command.mode = CommandModeCodes.PLANAR_ORI.value
self._command.x = copy.deepcopy(self._actual.x)
self._command.v = np.zeros(6)
self._command.x[3:] = rot_z(rtheta)
self._last_status = MotionResultCodes.MOTION_SUCCESS.value
_tstart = self.simtime()
while (self.simtime() - _tstart) < wait:
_eo = wrap_to_pi(rtheta - self.theta)
_u1 = 0
_u2 = k_rot * _eo
_ux = np.array([_u1, _u2])
_fac = max(max(_ux / self.v_max), max(_ux / self.v_min), 1)
_ux = _ux / _fac * vel_fac
self._command.v[:2] = q2r(self.x[3:])[:2, :2] @ np.array([_ux[0], 0.0])
self._command.v[5] = _ux[1]
self.Set_vel(_ux)
if abs(_eo) < ori_err:
break
self.Stop()
self.DebugMessage("CMoveToOri finished")
return self._last_status
[docs]
def CMoveToLocation(
self,
rp: Vector2DType,
rtheta: Optional[float] = None,
task_space: Optional[str] = None,
robot_as_a_sensor: bool = False,
min_dist: Optional[float] = None,
approach_dist: Optional[float] = None,
final_orientation_correction: bool = True,
wait: Optional[float] = None,
vel_fac: Optional[float] = None,
pos_err: Optional[float] = None,
ori_err: Optional[float] = None,
k_dist: Optional[float] = None,
k_dir: Optional[float] = None,
asynchronous: bool = False,
allow_backward: bool = False,
reach_check_fn: Optional[Callable[..., bool]] = None,
**kwargs: Any,
) -> Union[Thread, int]:
"""
Move the platform to a planar target location.
Parameters
----------
rp : Vector2DType
Target planar position ``[x, y]``. A three-element vector is also
accepted, in which case the third element is used as ``rtheta`` when
``rtheta`` is ``None``.
rtheta : float, optional
Target yaw angle in radians.
task_space : str, optional
Frame in which ``rp`` and ``rtheta`` are expressed. Supported values
are ``"World"``, ``"Object"``, and ``"Platform"``.
robot_as_a_sensor : bool, optional
If ``True``, use the attached robot pose to estimate the platform
approach state.
min_dist : float, optional
Distance threshold at which the approach path shaping starts.
approach_dist : float, optional
Distance used to shape the final approach direction.
final_orientation_correction : bool, optional
If ``True`` and ``rtheta`` is provided, run a final orientation
correction after reaching the position.
wait : float, optional
Maximum motion duration in seconds.
vel_fac : float, optional
Velocity scaling factor.
pos_err : float, optional
Position tolerance in meters.
ori_err : float, optional
Orientation tolerance in radians.
k_dist : float, optional
Proportional distance-control gain.
k_dir : float, optional
Proportional heading-control gain.
asynchronous : bool, optional
If ``True``, execute the motion in a daemon thread and return that
thread.
allow_backward : bool, optional
If ``True``, allow the controller to drive backward when the goal is
behind the platform.
reach_check_fn : Callable, optional
Callback used to stop the motion early. The callback receives the
attached robot and ``**kwargs``.
**kwargs : Any
Additional arguments passed to ``reach_check_fn``.
Returns
-------
Thread or int
The worker thread for asynchronous execution, otherwise a motion
result code.
"""
rp = vector(rp)
if isvector(rp, dim=2):
pass
elif isvector(rp, dim=3):
rtheta = rp[2] if rtheta is None else rtheta
rp = rp[:2]
if asynchronous:
self.DebugMessage("ASYNC CMoveToLocation")
_th = Thread(
target=self._CMoveToLocation,
args=(rp,),
kwargs={
"rtheta": rtheta,
"task_space": task_space,
"robot_as_a_sensor": robot_as_a_sensor,
"min_dist": min_dist,
"approach_dist": approach_dist,
"final_orientation_correction": final_orientation_correction,
"wait": wait,
"vel_fac": vel_fac,
"pos_err": pos_err,
"ori_err": ori_err,
"k_dist": k_dist,
"k_dir": k_dir,
"allow_backward": allow_backward,
"reach_check_fn": reach_check_fn,
**kwargs,
},
daemon=True,
)
_th.start()
return _th
else:
return self._CMoveToLocation(
rp,
rtheta=rtheta,
task_space=task_space,
robot_as_a_sensor=robot_as_a_sensor,
min_dist=min_dist,
approach_dist=approach_dist,
final_orientation_correction=final_orientation_correction,
wait=wait,
vel_fac=vel_fac,
pos_err=pos_err,
ori_err=ori_err,
k_dist=k_dist,
k_dir=k_dir,
allow_backward=allow_backward,
reach_check_fn=reach_check_fn,
**kwargs,
)
def _CMoveToLocation(
self,
rp: Vector2DType,
rtheta: Optional[float] = None,
task_space: Optional[str] = None,
robot_as_a_sensor: bool = False,
min_dist: Optional[float] = None,
approach_dist: Optional[float] = None,
final_orientation_correction: bool = True,
wait: Optional[float] = None,
vel_fac: Optional[float] = None,
pos_err: Optional[float] = None,
ori_err: Optional[float] = None,
k_dist: Optional[float] = None,
k_dir: Optional[float] = None,
min_vel: float = 0.02,
allow_backward: bool = False,
reach_check_fn: Optional[Callable[..., bool]] = None,
**kwargs: Any,
) -> int:
"""
Execute a blocking planar location move.
Parameters
----------
rp : Vector2DType
Target planar position ``[x, y]``.
rtheta : float, optional
Target yaw angle in radians.
task_space : str, optional
Frame in which ``rp`` and ``rtheta`` are expressed. Supported values
are ``"World"``, ``"Object"``, and ``"Platform"``.
robot_as_a_sensor : bool, optional
If ``True``, use the attached robot pose to estimate the platform
approach state.
min_dist : float, optional
Distance threshold at which the approach path shaping starts.
approach_dist : float, optional
Distance used to shape the final approach direction.
final_orientation_correction : bool, optional
If ``True`` and ``rtheta`` is provided, run a final orientation
correction after reaching the position.
wait : float, optional
Maximum motion duration in seconds.
vel_fac : float, optional
Velocity scaling factor.
pos_err : float, optional
Position tolerance in meters.
ori_err : float, optional
Orientation tolerance in radians.
k_dist : float, optional
Proportional distance-control gain.
k_dir : float, optional
Proportional heading-control gain.
min_vel : float, optional
Velocity threshold used to detect that the platform has effectively
stopped near the target.
allow_backward : bool, optional
If ``True``, allow the controller to drive backward when the goal is
behind the platform.
reach_check_fn : Callable, optional
Callback used to stop the motion early. The callback receives the
attached robot and ``**kwargs``.
**kwargs : Any
Additional arguments passed to ``reach_check_fn``.
Returns
-------
int
Motion result code.
Raises
------
ValueError
If ``task_space`` is not supported.
"""
if task_space is None:
task_space = self._default.TaskSpace
if wait is None:
wait = self._default.Wait
if min_dist is None:
min_dist = np.inf
if approach_dist is None:
approach_dist = self._default.ApproachDist
if vel_fac is None:
vel_fac = self._default.VelFac
if pos_err is None:
pos_err = self._default.PosErr
if k_dist is None:
k_dist = self._default.Kdist
if k_dir is None:
k_dir = self._default.Kdir
rp = vector(rp, dim=2)
if check_option(task_space, "World"):
pass
elif check_option(task_space, "Object"):
rp = self.ObjectToWorld(rp)
if rtheta is not None:
rtheta = self.ObjectToWorld(rtheta)
elif check_option(task_space, "Platform"):
rp = self.PlatformToWorld(rp)
if rtheta is not None:
rtheta = self.PlatformToWorld(rtheta)
else:
raise ValueError(f"Task space {task_space} not supported in CMoveToLocation")
self._command.x = copy.deepcopy(self._actual.x)
self._command.v = np.zeros(6)
if rtheta is not None:
self._command.x[3:] = rot_z(rtheta)
self._command.x[:2] = rp
_ee = rp - self._actual.x[:2]
if approach_dist == 0:
_beta = np.arctan2(_ee[1], _ee[0])
self.DebugMessage("CMoveToLocation -> CMoveToOri")
self._semaphore.release()
self._CMoveToOri(_beta, ori_err=ori_err)
if not self._semaphore.acquire(blocking=False):
self.WarningMessage("CMoveToLocation not executed due to active threads!")
return MotionResultCodes.ACTIVE_THREADS.value
if (wait == self._default.Wait) and (pos_err < np.inf):
wait = 100 * max(np.linalg.norm(_ee) / self.v_max[0] / vel_fac, 1)
_ang0 = None
_in_range = False
if not self._semaphore.acquire(blocking=False):
self.WarningMessage("CMoveToLocation not executed due to active threads!")
return MotionResultCodes.ACTIVE_THREADS.value
# self.DebugMessage(f"CMoveToLocation started: xy={rp} theta={rtheta:.3f}") TODO
if not self.Start():
self._semaphore.release()
return MotionResultCodes.NOT_READY.value
self._command.mode = CommandModeCodes.PLANAR_LOCATION.value
self._last_status = MotionResultCodes.MOTION_SUCCESS.value
_tstart = self.simtime()
_distance_to_target = np.linalg.norm(rp - self.p[:2])
# Procedure for cheking if we are in backward case
if robot_as_a_sensor:
_xrObj = np.linalg.inv(x2t(self.Robot._actual.x)) @ np.linalg.inv(self.TRobotBase)
_xaWorld = self.ObjectToWorld(t2x(_xrObj))
_xa = _xaWorld[:2]
_theta = np.arctan2(x2t(_xaWorld)[1, 0], x2t(_xaWorld)[0, 0])
else:
_xa = self.p[:2]
_theta = self.theta
vec_to_goal = rp - _xa # vector from robot to goal
head = np.array([np.cos(_theta), np.sin(_theta)]) # heading of robot
forward_dot = np.dot(vec_to_goal, head) # dot product: positive = front hemisphere, negative = rear hemisphere
backward_case = allow_backward and (forward_dot < 0) # enable backward if goal in rear hemisphere
while ((self.simtime() - _tstart) < wait) and not self._abort_motion:
if robot_as_a_sensor:
_xrObj = np.linalg.inv(x2t(self.Robot._actual.x)) @ np.linalg.inv(self.TRobotBase)
_xaWorld = self.ObjectToWorld(t2x(_xrObj))
_xa = _xaWorld[:2]
_theta = np.arctan2(x2t(_xaWorld)[1, 0], x2t(_xaWorld)[0, 0])
else:
_xa = self.p[:2]
_theta = self.theta
if backward_case:
_theta = wrap_to_pi(_theta + np.pi) # "virtual" angle for backward motion
if rtheta is None:
self._command.x[3:] = copy.deepcopy(self._actual.x[3:])
_xref = rp
else:
if backward_case:
_rtheta = wrap_to_pi(rtheta + np.pi) # "virtual" angle for appcoach
else:
_rtheta = rtheta
_dx = rp - _xa
_ndx = np.linalg.norm(_dx)
if _ndx < _eps:
_xref = rp
elif (_ndx < min_dist and approach_dist > 0) or _in_range:
_in_range = True
_beta = np.arctan2(_dx[1], _dx[0])
_gamma = min(max(wrap_to_pi(_beta - _rtheta), -np.pi / 4), np.pi / 4)
_phi = _beta + min(approach_dist / _ndx, 1) * _gamma
_xref = _xa + _ndx * np.array([np.cos(_phi), np.sin(_phi)])
else:
_xref = rp
_ee = _xref - _xa
_dist = np.linalg.norm(_ee)
_ang = np.arctan2(_ee[1], _ee[0])
if _ang0 is not None:
_dang = _ang - _ang0
if _dang > np.pi:
_ang -= 2 * np.pi
elif _dang < -np.pi:
_ang += 2 * np.pi
_ang0 = _ang
_alpha = wrap_to_pi(_ang - _theta)
_scale = min(_dist / min(pos_err, 0.1), 1.0)
_u1 = k_dist * max(np.cos(2 * _alpha), 0) * _scale * _dist
_u2 = k_dir * _scale * _alpha
if backward_case:
_u1 = -_u1 # physically we are moving backward
_ux = np.array([_u1, _u2])
_fac = max(max(_ux / self.v_max), max(_ux / self.v_min), 1)
_ux = _ux / _fac * vel_fac
self._command.v[:2] = q2r(self.x[3:])[:2, :2] @ np.array([_ux[0], 0.0])
self._command.v[5] = _ux[1]
self.Set_vel(_ux)
if (_dist < pos_err) or ((np.linalg.norm(_ee) < 0.1 * _distance_to_target) and (np.linalg.norm(self._actual.v[:2]) < min_vel)):
if rtheta is not None and final_orientation_correction:
self.DebugMessage("CMoveToLocation -> CMoveToOri")
self._semaphore.release()
self._CMoveToOri(rtheta, ori_err=ori_err)
if not self._semaphore.acquire(blocking=False):
self.WarningMessage("CMoveToLocation not executed due to active threads!")
return MotionResultCodes.ACTIVE_THREADS.value
break
if robot_as_a_sensor and reach_check_fn is not None:
if reach_check_fn(self.Robot, **kwargs):
self.DebugMessage("CMoveToLocation aborted by reach_check_fn")
break
self.Stop()
self.DebugMessage("CMoveToLocation finished")
return self._last_status
[docs]
def PForward(self, d: float, t: float = 1, traj: Optional[str] = None, asynchronous: bool = False) -> Optional[Union[Thread, int]]:
"""
Move the platform forward by a relative distance.
Parameters
----------
d : float
Relative forward distance in meters.
t : float, optional
Nominal motion duration in seconds.
traj : str, optional
Trajectory profile name. If ``None``, the default trajectory profile
is used.
asynchronous : bool, optional
If ``True``, execute the motion in a daemon thread and return that
thread.
Returns
-------
Thread or int or None
The worker thread for asynchronous execution, otherwise a motion
result code. ``None`` may be returned if the move is infeasible in
the base implementation.
"""
if asynchronous:
self.DebugMessage("ASYNC PForward")
_th = Thread(target=self._PForward, args=(d,), kwargs={"t": t, "traj": traj}, daemon=True)
_th.start()
return _th
else:
return self._PForward(d, t, traj=traj)
def _PForward(self, d: float, t: float = 1, traj: Optional[str] = None) -> Optional[int]:
"""
Execute a blocking relative forward move.
Parameters
----------
d : float
Relative forward distance in meters.
t : float, optional
Nominal motion duration in seconds.
traj : str, optional
Trajectory profile name. If ``None``, the default trajectory profile
is used.
Returns
-------
int or None
Motion result code. ``None`` may be returned if the requested motion
is infeasible.
"""
if not self._semaphore.acquire(blocking=False):
self.WarningMessage("PForward not executed due to active threads!")
return MotionResultCodes.ACTIVE_THREADS.value
if traj is None:
traj = self._default.Traj
self.DebugMessage("PForward started")
if not self.Start():
self._semaphore.release()
return MotionResultCodes.NOT_READY.value
self._command.mode = CommandModeCodes.PLANAR_FORWARD.value
self._last_status = MotionResultCodes.MOTION_SUCCESS.value
time = np.arange(self.tsamp, t + self.tsamp, self.tsamp)
self._command.x = copy.deepcopy(self._actual.x)
self._command.v = np.zeros(6)
p0 = self.p_ref[:2]
R0 = self.R_ref[:2, :2]
_di, _vi, _ = jtraj(0, d, time, traj=traj)
_fac = max(np.max(_vi, axis=0) / self.v_max[0], np.min(_vi, axis=0) / self.v_min[0])
if _fac > 1000:
self.Stop()
self.WarningMessage("PForward not possible")
return
elif _fac > 1:
time = np.arange(self.tsamp, t * _fac + self.tsamp, self.tsamp)
_di, _vi, _ = jtraj(0, d, time, traj=traj)
for _xt, _vt in zip(_di, _vi):
if self._do_motion_check and self._motion_check_callback is not None:
self._last_status = self._motion_check_callback(self)
if self._last_status != MotionResultCodes.MOTION_SUCCESS.value:
self.WarningMessage("Motion abborted")
break
self._command.x[:2] = p0 + R0 @ np.array([_xt, 0.0])
self._command.v[:2] = R0 @ np.array([_vt, 0.0])
self.Set_vel([_vt, 0])
if self._abort_motion:
self._last_status = MotionResultCodes.MOTION_ABORTED.value
break
self.Stop()
self.DebugMessage("PForward finished")
return self._last_status
[docs]
def PTurn(self, ang: float, t: float = 1, traj: Optional[str] = None, asynchronous: bool = False) -> Optional[Union[Thread, int]]:
"""
Rotate the platform by a relative planar angle.
Parameters
----------
ang : float
Relative rotation angle in radians.
t : float, optional
Nominal motion duration in seconds.
traj : str, optional
Trajectory profile name. If ``None``, the default trajectory profile
is used.
asynchronous : bool, optional
If ``True``, execute the motion in a daemon thread and return that
thread.
Returns
-------
Thread or int or None
The worker thread for asynchronous execution, otherwise a motion
result code. ``None`` may be returned if the turn is infeasible in
the base implementation.
"""
if asynchronous:
self.DebugMessage("ASYNC PTurn")
_th = Thread(target=self._PTurn, args=(ang,), kwargs={"t": t, "traj": traj}, daemon=True)
_th.start()
return _th
else:
return self._PTurn(ang, t, traj=traj)
def _PTurn(self, ang: float, t: float = 1, traj: Optional[str] = None) -> Optional[int]:
"""
Execute a blocking relative planar turn.
Parameters
----------
ang : float
Relative rotation angle in radians.
t : float, optional
Nominal motion duration in seconds.
traj : str, optional
Trajectory profile name. If ``None``, the default trajectory profile
is used.
Returns
-------
int or None
Motion result code. ``None`` may be returned if the requested motion
is infeasible.
"""
if not self._semaphore.acquire(blocking=False):
self.WarningMessage("PTurn not executed due to active threads!")
return MotionResultCodes.ACTIVE_THREADS.value
if traj is None:
traj = self._default.Traj
self.DebugMessage("PTurn started")
if not self.Start():
self._semaphore.release()
return MotionResultCodes.NOT_READY.value
self._command.mode = CommandModeCodes.PLANAR_TURN.value
self._last_status = MotionResultCodes.MOTION_SUCCESS.value
time = np.arange(self.tsamp, t + self.tsamp, self.tsamp)
self._command.x = copy.deepcopy(self._actual.x)
self._command.v = np.zeros(6)
theta0 = self.theta
_ai, _wi, _ = jtraj(0, ang, time, traj=traj)
_fac = max(np.max(_wi, axis=0) / self.v_max[1], np.min(_wi, axis=0) / self.v_min[1])
if _fac > 1000:
self.Stop()
self.WarningMessage("PTurn not possible")
return
elif _fac > 1:
time = np.arange(self.tsamp, t * _fac + self.tsamp, self.tsamp)
_ai, _wi, _ = jtraj(0, ang, time, traj=traj)
for _xt, _vt in zip(_ai, _wi):
if self._do_motion_check and self._motion_check_callback is not None:
self._last_status = self._motion_check_callback(self)
if self._last_status != MotionResultCodes.MOTION_SUCCESS.value:
self.WarningMessage("Motion abborted")
self._semaphore.release()
return self._last_status
self._command.x[3:] = rot_z(theta0 + _xt)
self._command.v[5] = _vt
self.Set_vel([0, _vt])
self.Stop()
self.DebugMessage("PTurn finished")
return self._last_status
[docs]
def AutonomousMotion(self, callback: Optional[Callable[..., int]] = None, asynchronous: bool = True, **kwargs: Any) -> Optional[Union[Thread, int]]:
"""
Execute an autonomous platform motion callback.
Parameters
----------
callback : Callable, optional
Function that performs the autonomous motion and returns a motion
result code.
asynchronous : bool, optional
If ``True``, execute the callback in a daemon thread and return that
thread.
**kwargs : Any
Additional keyword arguments passed to ``callback``.
Returns
-------
Thread or int or None
The worker thread for asynchronous execution, the callback result
code for blocking execution, or ``None`` if the callback cannot be
started.
"""
if callback is None:
self.ErrorMessage("No callback for autonomous motion defined!")
return
elif not callable(callback):
self.ErrorMessage("Parameter for autonomous motion is not a function!")
return
self._abort_autonomous_motion = False
if asynchronous:
self.DebugMessage("ASYNC AutonmousMotion")
_th = Thread(target=self._AutonomousMotion, args=(), kwargs={"callback": callback, **kwargs}, daemon=True)
_th.start()
return _th
else:
return self._AutonomousMotion(callback=callback, **kwargs)
def _AutonomousMotion(self, callback: Optional[Callable[..., int]] = None, **kwargs: Any) -> int:
"""
Execute a blocking autonomous motion callback.
Parameters
----------
callback : Callable, optional
Function that performs the autonomous motion and returns a motion
result code.
**kwargs : Any
Additional keyword arguments passed to ``callback``.
Returns
-------
int
Motion result code.
"""
if not self._semaphore.acquire(blocking=False):
self.WarningMessage("AutonomousMotion not executed due to active threads!")
return MotionResultCodes.ACTIVE_THREADS.value
self.DebugMessage("AutonmousMotion started")
if not self.Start():
self._semaphore.release()
return MotionResultCodes.NOT_READY.value
self._command.mode = CommandModeCodes.AUTONOMOUS.value
self._last_status = MotionResultCodes.MOTION_SUCCESS.value
self._last_status = callback(**kwargs)
self.Stop()
self.DebugMessage("AutonmousMotion finished")
return self._last_status
[docs]
def AbortAutonomousMotion(self) -> None:
"""
Request termination of the active autonomous motion.
Returns
-------
None
"""
self._abort_autonomous_motion = True
# Transformations
[docs]
def PlatformToWorld(self, x: ArrayLike, typ: Optional[str] = None) -> np.ndarray:
"""Map from platform base frame to world frame
Supported arguments: pose (7,), homogeneous matrix (4, 4), rotation matrix (3, 3),
position (3,), twist (6,) and JacobianType (6, nj)
Parameters
----------
x : ArrayLike
Value to map:
- pose (7,) or (4, 4)
- position (3, )
- orientation (4,) or (3, 3)
- velocity or force (6, )
- JacobianType (6, nj)
- 2D position (2,)
- rotation (1,)
typ : str, optional
Transformation type (None or ``Wrench``)
Returns
-------
array of floats
Mapped value.
Raises
------
ValueError
Parameter shape not supported
Notes
-----
2D position and scalar rotation can be used only if `z`-axis of both frames are colinear
"""
R0 = q2r(self._actual.x[3:])
p0 = self._actual.x[:3]
x = np.asarray(x)
if x.shape == (4, 4):
p, R = map_pose(T=x, out="pR")
return map_pose(p=R0 @ p + p0, R=R0 @ R, out="T")
elif isvector(x, dim=7):
p, R = map_pose(x=x, out="pR")
return map_pose(p=R0 @ p + p0, R=R0 @ R, out="x")
elif x.shape == (3, 3):
return R0 @ x
elif isvector(x, dim=4):
return r2q(R0 @ q2r(x))
elif isvector(x, dim=6):
RR = np.block([[R0, np.zeros((3, 3))], [np.zeros((3, 3)), R0]])
if typ == "Wrench": # wrench (F)
RR[3:6, :3] = v2s(p0) @ R0
return RR @ x
elif x.shape == (6, self.nj):
return np.vstack((R0 @ x[:3, :], R0 @ x[3:, :]))
elif isvector(x, dim=3):
return R0 @ x + p0
elif isvector(x, dim=2):
return R0[:2, :2] @ x + p0[:2]
elif isscalar(x):
return x + self.theta
else:
raise ValueError(f"Parameter shape {x.shape} not supported")
[docs]
def WorldToPlatform(self, x: ArrayLike, typ: Optional[str] = None) -> np.ndarray:
"""Map from world frame to platform base frame
Supported arguments: pose (7,), homogeneous matrix (4, 4), rotation matrix (3, 3),
position (3,), twist (6,) and JacobianType (6, nj)
Parameters
----------
x : ArrayLike
Value to map:
- pose (7,) or (4, 4)
- position (3, )
- orientation (4,) or (3, 3)
- velocity or force (6, )
- JacobianType (6, nj)
- 2D position (2,)
- rotation (1,)
typ : str, optional
Transformation type (None or ``Wrench``)
Returns
-------
array of floats
Mapped value.
Raises
------
ValueError
Parameter shape not supported
Notes
-----
2D position and scalar rotation can be used only if `z`-axis of both frames are colinear
"""
R0 = q2r(self._actual.x[3:]).T
p0 = -R0 @ self._actual.x[:3]
x = np.asarray(x)
if x.shape == (4, 4):
p, R = map_pose(T=x, out="pR")
return map_pose(p=R0 @ p + p0, R=R0 @ R, out="T")
elif isvector(x, dim=7):
p, R = map_pose(x=x, out="pR")
return map_pose(p=R0 @ p + p0, R=R0 @ R, out="x")
elif x.shape == (3, 3):
return R0 @ x
elif isvector(x, dim=4):
return r2q(R0 @ q2r(x))
elif isvector(x, dim=3):
return R0 @ x + p0
elif isvector(x, dim=6):
RR = np.block([[R0, np.zeros((3, 3))], [np.zeros((3, 3)), R0]])
if typ == "Wrench": # wrench (F)
RR[3:6, :3] = v2s(p0) @ R0
return RR @ x
elif x.shape == (6, self.nj):
return np.vstack((R0 @ x[:3, :], R0 @ x[3:, :]))
elif isvector(x, dim=3):
return R0 @ x + p0
elif isvector(x, dim=2):
return R0[:2, :2] @ x + p0[:2]
elif isscalar(x):
return x - self.theta
else:
raise ValueError(f"Parameter shape {x.shape} not supported")
[docs]
def ObjectToWorld(self, x: ArrayLike, typ: Optional[str] = None) -> np.ndarray:
"""Map from object frame to world frame
Supported arguments: pose (7,), homogeneous matrix (4, 4), rotation matrix (3, 3),
position (3,), twist (6,) and JacobianType (6, nj)
Parameters
----------
x : ArrayLike
Value to map:
- pose (7,) or (4, 4)
- position (3, )
- orientation (4,) or (3, 3)
- velocity or force (6, )
- JacobianType (6, nj)
- 2D position (2,)
- rotation (1,)
typ : str, optional
Transformation type (None or ``Wrench``)
Returns
-------
array of floats
Mapped value.
Raises
------
ValueError
Parameter shape not supported
Notes
-----
2D position and scalar rotation can be used only if `z`-axis of both frames are colinear
"""
R0 = self.TObject[:3, :3]
p0 = self.TObject[:3, 3]
x = np.asarray(x)
if x.shape == (4, 4):
p, R = map_pose(T=x, out="pR")
return map_pose(p=R0 @ p + p0, R=R0 @ R, out="T")
elif isvector(x, dim=7):
p, R = map_pose(x=x, out="pR")
return map_pose(p=R0 @ p + p0, R=R0 @ R, out="x")
elif x.shape == (3, 3):
return R0 @ x
elif isvector(x, dim=4):
return r2q(R0 @ q2r(x))
elif isvector(x, dim=6):
RR = np.block([[R0, np.zeros((3, 3))], [np.zeros((3, 3)), R0]])
# if typ == "Wrench": # wrench (F)
# RR[3:6, :3] = v2s(p0) @ R0
return RR @ x
elif x.shape == (6, self.nj):
return np.vstack((R0 @ x[:3, :], R0 @ x[3:, :]))
elif isvector(x, dim=3):
return R0 @ x + p0
elif isvector(x, dim=2):
return R0[:2, :2] @ x + p0[:2]
elif isscalar(x):
R_obj = rot_z(x, out="R")
R_world = R0 @ R_obj
theta_world = np.arctan2(R_world[1, 0], R_world[0, 0])
return theta_world
else:
raise ValueError(f"Parameter shape {x.shape} not supported")
[docs]
def WorldToObject(self, x: ArrayLike, typ: Optional[str] = None) -> np.ndarray:
"""Map from world frame to object frame
Supported arguments: pose (7,), homogeneous matrix (4, 4), rotation matrix (3, 3),
position (3,), twist (6,) and JacobianType (6, nj)
Parameters
----------
x : ArrayLike
Value to map:
- pose (7,) or (4, 4)
- position (3, )
- orientation (4,) or (3, 3)
- velocity or force (6, )
- JacobianType (6, nj)
- 2D position (2,)
- rotation (1,)
typ : str, optional
Transformation type (None or ``Wrench``)
Returns
-------
array of floats
Mapped value.
Raises
------
ValueError
Parameter shape not supported
Notes
-----
2D position and scalar rotation can be used only if `z`-axis of both frames are colinear
"""
R0 = self.TObject[:3, :3].T
p0 = -R0 @ self.TObject[:3, 3]
x = np.asarray(x)
if x.shape == (4, 4):
p, R = map_pose(T=x, out="pR")
return map_pose(p=R0 @ p + p0, R=R0 @ R, out="T")
elif isvector(x, dim=7):
p, R = map_pose(x=x, out="pR")
return map_pose(p=R0 @ p + p0, R=R0 @ R, out="x")
elif x.shape == (3, 3):
return R0 @ x
elif isvector(x, dim=4):
return r2q(R0 @ q2r(x))
elif isvector(x, dim=6):
RR = np.block([[R0, np.zeros((3, 3))], [np.zeros((3, 3)), R0]])
# if typ == "Wrench": # wrench (F)
# RR[3:6, :3] = v2s(p0) @ R0
return RR @ x
elif x.shape == (6, self.nj):
return np.vstack((R0 @ x[:3, :], R0 @ x[3:, :]))
elif isvector(x, dim=3):
return R0 @ x + p0
elif isvector(x, dim=2):
return R0[:2, :2] @ x + p0[:2]
elif isscalar(x):
R_world = rot_z(x, out="R")
R_obj = R0 @ R_world
return np.arctan2(R_obj[1, 0], R_obj[0, 0])
else:
raise ValueError(f"Parameter shape {x.shape} not supported")
# Kinematic utilities
[docs]
def Kinmodel(self, q: Optional[ArrayLike] = None, out: str = "x") -> Tuple[np.ndarray, JacobianType]:
"""
Compute the base platform kinematic model.
Parameters
----------
q : ArrayLike, optional
Platform pose or configuration used for the kinematic model. If
``None``, the current platform pose is used.
out : str, optional
Pose output format passed to :func:`map_pose`.
Returns
-------
tuple of np.ndarray and JacobianType
Platform pose in the requested output format and the corresponding
Jacobian matrix.
"""
if q is None:
_q = self.x
else:
_q = q
J = np.zeros((6, 2))
J[0, 0] = 1
J[0, 1] = 1
J[5, 0] = -1
J[5, 1] = 1
return map_pose(x=_q, out=out), J
[docs]
def Jacobi(self) -> JacobianType:
"""
Get the platform Jacobian at the current pose.
Returns
-------
JacobianType
Platform Jacobian matrix.
"""
km = self.Kinmodel()
return km[-1]
# Object
[docs]
def SetObject(self, x: Optional[Union[Pose3DType, HomogeneousMatrixType]] = None) -> None:
"""
Set the object pose in the platform coordinate system.
Parameters
----------
x : Union[Pose3DType, HomogeneousMatrixType], optional
The object pose as a pose vector ``(7,)`` or homogeneous matrix ``(4, 4)``.
If ``None``, the current actual platform pose is used.
Returns
-------
None
Raises
------
ValueError
If the pose shape is not supported or if the input pose z-axis is not aligned
with ``[0, 0, 1]``.
"""
if x is None:
_x = self._actual.x
else:
_x = self.spatial(x)
if _x.shape == (4, 4):
_T = _x
elif isvector(_x, dim=7):
_T = x2t(_x)
else:
raise ValueError(f"Object pose shape {_x.shape} not supported")
_z_axis = _T[:3, 2]
if not np.allclose(_z_axis, np.array([0.0, 0.0, 1.0]), atol=_eps, rtol=0.0):
raise ValueError(f"Object pose z-axis must equal [0, 0, 1], got {_z_axis}")
self.TObject = _T
[docs]
def GetObject(self, out: str = "T", task_space: Optional[str] = None) -> Union[Pose3DType, HomogeneousMatrixType, Vector3DType, QuaternionType, RotationMatrixType]:
"""
Get the object pose in the specified task space.
Parameters
----------
out : str, optional
The output format of the object pose. Default is ``"T"``.
task_space : str, optional
The task space for the pose transformation. Supported values are
``"World"``, ``"Object"``, and ``"Platform"``.
Returns
-------
Pose3DType or HomogeneousMatrixType or Vector3DType or QuaternionType or RotationMatrixType
The object pose in the requested output format.
Raises
------
ValueError
If the task space is not supported.
"""
if task_space is None:
task_space = self._default.TaskSpace
_T = self.TObject
if check_option(task_space, "World"):
pass
elif check_option(task_space, "Object"):
_T = self.WorldToObject(_T)
elif check_option(task_space, "Platform"):
_T = self.WorldToPlatform(_T)
else:
raise ValueError(f"Task space {task_space} not supported in GetObject")
return map_pose(T=_T, out=out)
# Robot base
[docs]
def AttachTo(self, robot: robot) -> None:
"""
Attach a robot instance to the platform.
Parameters
----------
robot : robot
Robot instance to associate with this platform.
Returns
-------
None
"""
self.Robot = robot
[docs]
def Detach(self) -> None:
"""
Detach the currently attached robot from the platform.
Returns
-------
None
"""
self.Robot = None
[docs]
def GetAttachedRobot(self) -> Optional["robot"]:
"""
Get the robot attached to platform.
Returns
-------
Optional[robot]
The attached robot instance, or ``None``.
"""
if self.Robot is None:
return None
else:
return self.Robot
[docs]
def SetRobotBase(self, x: ArrayLike) -> None:
"""
Set the robot base pose relative to the platform.
Parameters
----------
x : ArrayLike
The robot base pose as a homogeneous matrix, pose vector, position,
rotation matrix, or quaternion.
Returns
-------
None
Raises
------
ValueError
If the base pose shape is not supported.
"""
x = self.spatial(x)
if x.shape == (4, 4):
_T = x
elif x.shape == (3, 3):
_T = map_pose(R=x, out="T")
elif isvector(x, dim=7):
_T = map_pose(x=x, out="T")
elif isvector(x, dim=3):
_T = map_pose(p=x, out="T")
elif isvector(x, dim=4):
_T = map_pose(Q=x, out="T")
else:
raise ValueError(f"Robot base frame shape {x.shape} not supported")
self.TRobotBase = _T
[docs]
def GetRobotBase(self, out: str = "T") -> Union[Pose3DType, HomogeneousMatrixType, Vector3DType, QuaternionType, RotationMatrixType]:
"""
Get the robot base pose relative to the platform.
Parameters
----------
out : str, optional
The output format of the base pose. Default is ``"T"``.
Returns
-------
Pose3DType or HomogeneousMatrixType or Vector3DType or QuaternionType or RotationMatrixType
The robot base pose in the requested output format.
"""
if out is None:
out = self._default.TaskPoseForm
return map_pose(T=self.TRobotBase, out=out)
[docs]
def GetRobotBasePose(self, out: Optional[str] = None) -> Union[Pose3DType, HomogeneousMatrixType, Vector3DType, QuaternionType, RotationMatrixType]:
"""
Get the attached robot base pose in the world frame.
Parameters
----------
out : str, optional
The output format of the base pose. If ``None``, the default task pose
format is used.
Returns
-------
Pose3DType or HomogeneousMatrixType or Vector3DType or QuaternionType or RotationMatrixType
The robot base pose in the world frame.
"""
if out is None:
out = self._default.TaskPoseForm
self.GetState()
return map_pose(T=self.T @ self.TRobotBase, out=out)
# Movements
[docs]
def Start(self) -> bool:
"""
Start platform motion execution.
Returns
-------
bool
``True`` if the platform is ready and motion execution was started,
otherwise ``False``.
"""
if self.HasError():
self.WarningMessage("Platform in error mode. Can not start!")
return False
if self.isActive():
self._abort_motion = False
else:
self._abort_motion = True
self.WarningMessage("Not started due to inactive platform!")
return False
self._command.mode = CommandModeCodes.START.value
self._last_control_time = self.simtime()
self._abort_motion = False
self._motion_error = None
self.Update()
return True
[docs]
def Stop(self) -> None:
"""
Stop the active platform motion.
Returns
-------
None
"""
self._command.mode = CommandModeCodes.STOP.value
self._command.qdot = np.zeros(self.nj)
self._command.v = np.zeros(6)
self._abort_motion = False
self._motion_error = None
self.reset_threads()
self.Update()
[docs]
def Abort(self, abort: bool = True) -> None:
"""
Abort or release the current platform motion.
Parameters
----------
abort : bool, optional
If ``True``, request motion abort and switch the command mode to
:attr:`CommandModeCodes.ABORT.value`. If ``False``, clear the abort
flag.
Returns
-------
None
"""
if abort and self._command.mode <= CommandModeCodes.STOP.value:
self.WarningMessage(f"Motiom can not be aborted. Current motion state: {CommandModeStr(self._command.mode)}")
else:
self.DebugMessage(f"Abort: {abort}")
self._abort_motion = abort
if abort:
self._command.mode = CommandModeCodes.ABORT.value
self.Update()
[docs]
def WaitUntilStopped(self, eps: float = 0.001) -> None:
"""
Block until the platform joint velocity norm is below a threshold.
Parameters
----------
eps : float, optional
Velocity norm threshold used to decide that the platform has
stopped.
Returns
-------
None
"""
self.GetState()
while np.linalg.norm(self._actual.qdot) > eps:
self.GetState()
[docs]
def Wait(self, wait: float, dt: Optional[float] = None) -> Optional[int]:
"""
Hold the current command mode for a fixed duration.
Parameters
----------
wait : float
Duration in seconds.
dt : float, optional
Polling interval in seconds. If ``None``, :attr:`tsamp` is used.
Returns
-------
int or None
``MotionResultCodes.ACTIVE_THREADS.value`` if another threaded
motion is active, otherwise ``None``.
"""
if not self._semaphore.acquire(blocking=False):
self.WarningMessage("Wait not executed due to active threads!")
return MotionResultCodes.ACTIVE_THREADS.value
self.DebugMessage(f"Wait for {wait} s")
if dt is None:
dt = self.tsamp
tx = self.simtime()
imode = self._command.mode
self._command.mode = CommandModeCodes.WAIT.value
while self.simtime() - tx < wait:
self._sleep(dt)
self.GetState()
self.Update()
self._command.mode = imode
self._semaphore.release()
[docs]
def Restart(self) -> None:
"""
Stop and then start platform motion execution.
Returns
-------
None
"""
self.Stop()
self.Start()
[docs]
def SetMotionCheckCallback(self, fun: Callable[..., int]) -> None:
"""
Set the motion-check callback.
Parameters
----------
fun : Callable
Callback invoked during supported motion loops. It should return a
motion result code.
Returns
-------
None
"""
self._motion_check_callback = fun
[docs]
def EnableMotionCheck(self, check: bool = True) -> None:
"""
Enable or disable motion-check callbacks.
Parameters
----------
check : bool, optional
If ``True``, enable motion checks. If ``False``, disable them.
Returns
-------
None
"""
self._do_motion_check = check
[docs]
def DisableMotionCheck(self) -> None:
"""
Disable motion-check callbacks.
Returns
-------
None
"""
self._do_motion_check = False
# Utilities
[docs]
def SetCaptureCallback(self, fun: Callable[..., None]) -> None:
"""
Set the callback used during capture updates.
Parameters
----------
fun : Callable
Callback invoked with this platform instance during capture updates.
Returns
-------
None
"""
self._capture_callback = fun
[docs]
def StartCapture(self) -> None:
"""
Enable capture callbacks during state updates.
Returns
-------
None
"""
if not self._do_update:
self.WarningMessage("Update is not enabled")
# self._t0 = self._tt
self._do_capture = True
self.Update()
[docs]
def StopCapture(self) -> None:
"""
Disable capture callbacks during state updates.
Returns
-------
None
"""
self._do_capture = False
[docs]
def SetUserData(self, data: Optional[Any]) -> None:
"""
Store user-defined data in the command state.
Parameters
----------
data : Any, optional
User data to store.
Returns
-------
None
"""
self._command.data = data
self.Update()
[docs]
def GetUserData(self) -> Optional[Any]:
"""
Get user-defined data stored in the command state.
Returns
-------
Any or None
Stored user data, or ``None`` if no data is stored.
"""
return self._command.data
[docs]
def isplatform(obj: object) -> bool:
"""
Check whether an object is a platform instance.
Parameters
----------
obj : object
Object to test.
Returns
-------
bool
``True`` if `obj` is an instance of :class:`platform`, otherwise ``False``.
"""
return isinstance(obj, platform)