rby1_sdk.dynamics<a class="headerlink" href="#module-rby1_sdk.dynamics" title="Link to this heading">#

Notes

compute_forward_kinematics must be called before this function.

compute_body_velocity(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 1]]#

Computes the relative body velocity (twist) of a target link with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6x1 body velocity vector (twist).

Return type:

Notes

compute_forward_kinematics and compute_diff_forward_kinematics must be called before this function.

compute_center_of_mass(*args, **kwargs)#

Overloaded function.

compute_center_of_mass(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, ref_link: int, target_link: int) -> numpy.ndarray[numpy.float64[3, 1]]

Computes the center of mass of a single target link with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.
target_link (int) – The index of the target link.

Returns:

The 3D position vector of the center of mass.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, ref_link: int, target_links: list[int]) -> numpy.ndarray[numpy.float64[3, 1]]

Computes the combined center of mass of multiple target links with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.
target_links (list[int]) – A list of indices of the target links.

Returns:

The 3D position vector of the combined center of mass.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, ref_link: int) -> numpy.ndarray[numpy.float64[3, 1]]

Computes the center of mass of the entire robot with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.

Returns:

The 3D position vector of the total center of mass.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass_jacobian(*args, **kwargs)#

Overloaded function.

compute_center_of_mass_jacobian(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, ref_link: int, target_link: int) -> numpy.ndarray[numpy.float64[3, n]]

Computes the Jacobian for the center of mass of a single target link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.
target_link (int) – The index of the target link.

Returns:

The 3xDOF center of mass Jacobian matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass_jacobian(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, ref_link: int) -> numpy.ndarray[numpy.float64[3, n]]

Computes the Jacobian for the center of mass of the entire robot.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.

Returns:

The 3xDOF center of mass Jacobian matrix for the whole robot.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_diff_forward_kinematics(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State) → None#

Computes the differential forward kinematics for each joint.

This method calculates the body velocity (twist) for each joint frame based on the current joint velocities (qdot) in the state. The results are cached within the state object.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, which includes joint velocities. This object will be updated with the computed body velocities.

Notes

compute_forward_kinematics must be called before this function.

Examples

>>> # Continuing from the previous example...
>>> dyn_state.set_qdot(np.zeros(dyn_robot.get_dof()))
>>> dyn_robot.compute_diff_forward_kinematics(dyn_state)

compute_forward_kinematics(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State) → None#

Computes the forward kinematics for each joint.

This method calculates the transformation matrix from the base to each joint frame based on the current joint positions (q) in the state. The results are cached within the state object. This function must be called before other kinematics or dynamics calculations.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, which includes joint positions. This object will be updated with the computed transformation matrices.

Examples

>>> import rby1_sdk.dynamics as rby_dyn
>>> import numpy as np
>>> # Assume dyn_robot is an initialized rby_dyn.Robot instance
>>> # and dyn_state is a corresponding state object.
>>> dyn_state.set_q(np.random.rand(dyn_robot.get_dof()))
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> # Now you can compute transformations, Jacobians, etc.
>>> transform = dyn_robot.compute_transformation(dyn_state, 0, 1)
>>> print(transform)

compute_gravity_term(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State) → numpy.ndarray[numpy.float64[m, 1]]#

Computes the gravity compensation term for the robot.

This method calculates the joint torques required to counteract gravity at the current joint positions. The gravity vector must be set in the state object prior to calling this function.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions and the gravity vector.
Returns:: A vector of joint torques required to compensate for gravity.
Return type:: numpy.ndarray

Notes

compute_forward_kinematics must be called before this function.
The gravity vector (spatial acceleration) must be set on the state object using state.set_gravity() or state.set_Vdot0(). For standard gravity along the negative Z-axis, the vector is [0, 0, 0, 0, 0, -9.81].

Examples

>>> # Continuing from a previous example where dyn_robot and dyn_state are set up.
>>> dyn_state.set_gravity(np.array([0, 0, 0, 0, 0, -9.81]))
>>> # or dyn_state.set_Vdot0(np.array([0, 0, 0, 0, 0, 9.81]))  # Note that direction is reversed
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> gravity_torques = dyn_robot.compute_gravity_term(dyn_state)
>>> print(gravity_torques)

compute_inverse_dynamics(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State) → None#

Computes the inverse dynamics of the robot.

This method calculates the joint torques required to achieve the given joint accelerations (qddot), considering the current joint positions (q) and velocities (qdot). The results are stored back into the state object.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions, velocities, and accelerations. This object will be updated with the computed joint torques. Hello. This is state.

Notes

compute_forward_kinematics, compute_diff_forward_kinematics, and compute_2nd_diff_forward_kinematics must be called in order before this function.

Examples

>>> # This example demonstrates the full sequence for inverse dynamics.
>>> import rby1_sdk as rby
>>> import rby1_sdk.dynamics as rby_dyn
>>> import numpy as np
>>> robot = rby.create_robot_a("localhost:50051")
>>> robot.connect()
>>> dyn_robot = robot.get_dynamics()
>>> dyn_state = dyn_robot.make_state(
...     dyn_robot.get_link_names(), dyn_robot.get_joint_names()
... )
>>> q = (np.random.rand(dyn_robot.get_dof()) - 0.5) * np.pi / 2
>>> dyn_state.set_q(q)
>>> dyn_state.set_qdot(np.zeros(dyn_robot.get_dof()))
>>> dyn_state.set_qddot(np.zeros(dyn_robot.get_dof()))
>>>
>>> # Perform kinematics calculations in order
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> dyn_robot.compute_diff_forward_kinematics(dyn_state)
>>> dyn_robot.compute_2nd_diff_forward_kinematics(dyn_state)
>>>
>>> # Compute inverse dynamics
>>> dyn_robot.compute_inverse_dynamics(dyn_state)
>>>
>>> # Get the resulting torques
>>> torques = dyn_state.get_tau()
>>> with np.printoptions(precision=4, suppress=True):
...     print(f"Inverse dynamics torque (Nm): {torques}")

compute_mass(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, target_link_index: int) → float#

Computes the mass of a specific link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
target_link_index (int) – The index of the target link.

Returns:

The mass of the specified link.

Return type:

compute_mass_matrix(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State) → numpy.ndarray[numpy.float64[m, n]]#

Computes the joint space mass matrix (inertia matrix) of the robot.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions.
Returns:: The mass matrix (a square matrix of size DOF x DOF).
Return type:: numpy.ndarray

Notes

compute_forward_kinematics must be called before this function.

compute_mobility_diff_kinematics(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State) → numpy.ndarray[numpy.float64[3, 1]]#

Computes the forward differential kinematics for the mobile base.

Calculates the linear and angular velocity of the mobile base from the current wheel velocities (qdot).

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including wheel velocities.
Returns:: The resulting body velocity vector [w, vx, vy].
Return type:: numpy.ndarray

compute_mobility_inverse_diff_kinematics(*args, **kwargs)#

Overloaded function.

compute_mobility_inverse_diff_kinematics(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, linear_velocity: numpy.ndarray[numpy.float64[2, 1]], angular_velocity: float) -> None

Computes the inverse differential kinematics for the mobile base.

Calculates the required wheel velocities to achieve a desired linear and angular velocity of the mobile base. Updates the qdot values in the state object.

Parameters:

state (rby1_sdk.dynamics.State) – The robot state object to be updated.
linear_velocity (numpy.ndarray) – The desired linear velocity (x, y) [m/s].
angular_velocity (float) – The desired angular velocity (yaw) [rad/s].

compute_mobility_inverse_diff_kinematics(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, body_velocity: numpy.ndarray[numpy.float64[3, 1]]) -> None

Computes the inverse differential kinematics for the mobile base from a body velocity vector.

Calculates the required wheel velocities from a desired body velocity (twist). Updates the qdot values in the state object.

Parameters:

state (rby1_sdk.dynamics.State) – The robot state object to be updated.
body_velocity (numpy.ndarray) – The desired body velocity vector [w, vx, vy].

compute_reflective_inertia(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 6]]#

Computes the reflective inertia (task space inertia) of the target link with respect to the reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6x6 reflective inertia matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_space_jacobian(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, n]]#

Computes the space Jacobian for a target link relative to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6xDOF space Jacobian matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_total_inertial(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, ref_link: int) → numpy.ndarray[numpy.float64[6, 6]]#

Computes the total spatial inertia of the entire robot with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.

Returns:

The 6x6 total spatial inertia matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_transformation(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[4, 4]]#

Computes the transformation matrix from a reference link to a target link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link (the ‘from’ frame).
target_link_index (int) – The index of the target link (the ‘to’ frame).

Returns:

The 4x4 transformation matrix (SE(3)).

Return type:

Notes

compute_forward_kinematics must be called before this function.

static count_joints(base_link: rby1_sdk.dynamics.Link, include_fixed: bool = False) → int#

count_joints(base_link, include_fixed=False)

Counts the number of joints in a kinematic chain starting from a base link.

Parameters:

base_link (rby1_sdk.dynamics.Link) – The starting link of the kinematic chain.
include_fixed (bool, optional) – Whether to include fixed joints in the count. Default is False.

Returns:

The total number of joints.

Return type:

detect_collisions_or_nearest_links(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, collision_threshold: int = 0) → list[rby1_sdk.dynamics.CollisionResult]#

Detects collisions or finds the nearest links in the robot model.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
collision_threshold (int, optional) – The minimum number of link pairs to return. The function first finds all colliding pairs. If the number of colliding pairs is less than this threshold, it will supplement the result with the nearest non-colliding link pairs until the total count reaches the threshold. The returned list is always sorted by distance. If set to 0, only actual collisions are returned. Default is 0.

Returns:

A list of collision results, sorted by distance.

Return type:

Notes

compute_forward_kinematics must be called before this function.

get_base(self: rby1_sdk.dynamics.Robot) → rby1_sdk.dynamics.Link#

Get the base link of the robot.

Returns:: Base link.
Return type:: Link

get_dof(self: rby1_sdk.dynamics.Robot) → int#

Get the number of degrees of freedom.

Returns:: Number of degrees of freedom.
Return type:: int

get_joint_names(self: rby1_sdk.dynamics.Robot) → list[str]#

Get the list of names of all joints.

Returns:: List of joint names.
Return type:: list

get_joint_property(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, getter: Callable[[rby1_sdk.dynamics.Joint], float]) → numpy.ndarray[numpy.float64[m, 1]]#

Gets a specific property for all joints using a provided getter function.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
getter (callable) – A function that takes a joint object and returns a double value.

Returns:

A vector containing the specified property for each joint.

Return type:

get_limit_q_lower(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State) → numpy.ndarray[numpy.float64[m, 1]]#

Gets the lower position limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of lower position limits (q) for each joint.
Return type:: numpy.ndarray

get_limit_q_upper(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State) → numpy.ndarray[numpy.float64[m, 1]]#

Gets the upper position limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of upper position limits (q) for each joint.
Return type:: numpy.ndarray

get_limit_qddot_lower(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State) → numpy.ndarray[numpy.float64[m, 1]]#

Gets the lower acceleration limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of lower acceleration limits (q_ddot) for each joint.
Return type:: numpy.ndarray

get_limit_qddot_upper(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State) → numpy.ndarray[numpy.float64[m, 1]]#

Gets the upper acceleration limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of upper acceleration limits (q_ddot) for each joint.
Return type:: numpy.ndarray

get_limit_qdot_lower(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State) → numpy.ndarray[numpy.float64[m, 1]]#

Gets the lower velocity limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of lower velocity limits (q_dot) for each joint.
Return type:: numpy.ndarray

get_limit_qdot_upper(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State) → numpy.ndarray[numpy.float64[m, 1]]#

Gets the upper velocity limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of upper velocity limits (q_dot) for each joint.
Return type:: numpy.ndarray

get_limit_torque(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State) → numpy.ndarray[numpy.float64[m, 1]]#

Gets the torque limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of torque limits for each joint.
Return type:: numpy.ndarray

get_link(*args, **kwargs)#

Overloaded function.

get_link(self: rby1_sdk.dynamics.Robot, name: str) -> rby1_sdk.dynamics.Link

Get a link by name.

Parameters:: name (str) – Name of the link.
Returns:: Link if found, None otherwise.
Return type:: Link, optional

get_link(self: rby1_sdk.dynamics.Robot, state: rby1_sdk.dynamics.State, index: int) -> rby1_sdk.dynamics.Link

Get a link by state and index.

Parameters:

state (State) – Current state of the robot.
index (int) – Index of the link.

Returns:

Link if found, None otherwise.

Return type:

Link, optional

get_link_names(self: rby1_sdk.dynamics.Robot) → list[str]#

Get the list of names of all links.

Returns:: List of link names.
Return type:: list

get_number_of_joints(self: rby1_sdk.dynamics.Robot) → int#

Get the number of joints.

Returns:: Number of joints.
Return type:: int

make_state(self: rby1_sdk.dynamics.Robot, link_names: list[str], joint_names: list[str]) → rby1_sdk.dynamics.State#

Create a state from link and joint names.

The state object is essential for using the robot dynamics functions. It stores the robot’s state, its state vector (e.g., indices of joints and links), and also serves as a cache for intermediate results in dynamics and kinematics calculations to optimize for speed.

Parameters:

link_names (list[str]) – List of link names.
joint_names (list[str]) – List of joint names.

Returns:

A new state object.

Return type:

Examples

>>> import rby1_sdk.dynamics as rby_dyn
>>> import numpy as np
>>>
>>> link_0 = rby_dyn.Link("link_0")
>>> link_1 = rby_dyn.Link("link_1")
>>>
>>> joint_0 = rby_dyn.Joint.make_revolute("joint_0", np.identity(4), np.array([0, 0, 1]))
>>> joint_0.connect_links(link_0, link_1, np.identity(4), np.identity(4))
>>>
>>> dyn_robot = rby_dyn.Robot(
...     rby_dyn.RobotConfiguration(name="sample_robot", base_link=link_0)
... )
>>>
>>> dyn_state = dyn_robot.make_state(["link_0", "link_1"], ["joint_0"])
>>> dyn_state.set_q(np.array([np.pi / 2]))  # Angle of joint_0 is 90 degrees
>>>
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> # Calculate transformation from link_0 to link_1
>>> transform = dyn_robot.compute_transformation(dyn_state, 0, 1)  # 0: link_0, 1: link_1
>>> print(transform)

class RobotConfiguration#

Bases: pybind11_object

Robot configuration.

Defines the base link and mobile base of the robot.

name#

Name of the robot configuration.

Type:: str

base_link#

Base link of the robot.

Type:: Link

mobile_base#

Mobile base of the robot.

Type:: MobileBase

property base_link#

property mobile_base#

property name#

class Robot_18#

Bases: pybind11_object

Robot (DOF=18) dynamics model.

Represents the dynamics of a robot with a given number of degrees of freedom.

base#

Base link of the robot.

Type:: Link

link_names#

List of names of all links.

Type:: list

joint_names#

List of names of all joints.

Type:: list

compute_2nd_diff_forward_kinematics(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → None#

Computes the second-order differential forward kinematics for each joint.

This method calculates the body acceleration for each joint frame based on the current joint accelerations (qddot) in the state. The results are cached within the state object.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions, velocities, and accelerations. This object will be updated with the computed body accelerations.

Notes

compute_forward_kinematics and compute_diff_forward_kinematics must be called before this function.

Examples

>>> # Continuing from the previous example...
>>> dyn_state.set_qddot(np.zeros(dyn_robot.get_dof()))
>>> dyn_robot.compute_2nd_diff_forward_kinematics(dyn_state)

compute_body_jacobian(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 18]]#

Computes the body Jacobian for a target link relative to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6xDOF body Jacobian matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_body_velocity(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 1]]#

Computes the relative body velocity (twist) of a target link with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6x1 body velocity vector (twist).

Return type:

Notes

compute_forward_kinematics and compute_diff_forward_kinematics must be called before this function.

compute_center_of_mass(*args, **kwargs)#

Overloaded function.

compute_center_of_mass(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, ref_link: int, target_link: int) -> numpy.ndarray[numpy.float64[3, 1]]

Computes the center of mass of a single target link with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.
target_link (int) – The index of the target link.

Returns:

The 3D position vector of the center of mass.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, ref_link: int, target_links: list[int]) -> numpy.ndarray[numpy.float64[3, 1]]

Computes the combined center of mass of multiple target links with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.
target_links (list[int]) – A list of indices of the target links.

Returns:

The 3D position vector of the combined center of mass.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, ref_link: int) -> numpy.ndarray[numpy.float64[3, 1]]

Computes the center of mass of the entire robot with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.

Returns:

The 3D position vector of the total center of mass.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass_jacobian(*args, **kwargs)#

Overloaded function.

compute_center_of_mass_jacobian(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, ref_link: int, target_link: int) -> numpy.ndarray[numpy.float64[3, 18]]

Computes the Jacobian for the center of mass of a single target link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.
target_link (int) – The index of the target link.

Returns:

The 3xDOF center of mass Jacobian matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass_jacobian(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, ref_link: int) -> numpy.ndarray[numpy.float64[3, 18]]

Computes the Jacobian for the center of mass of the entire robot.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.

Returns:

The 3xDOF center of mass Jacobian matrix for the whole robot.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_diff_forward_kinematics(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → None#

Computes the differential forward kinematics for each joint.

This method calculates the body velocity (twist) for each joint frame based on the current joint velocities (qdot) in the state. The results are cached within the state object.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, which includes joint velocities. This object will be updated with the computed body velocities.

Notes

compute_forward_kinematics must be called before this function.

Examples

>>> # Continuing from the previous example...
>>> dyn_state.set_qdot(np.zeros(dyn_robot.get_dof()))
>>> dyn_robot.compute_diff_forward_kinematics(dyn_state)

compute_forward_kinematics(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → None#

Computes the forward kinematics for each joint.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, which includes joint positions. This object will be updated with the computed transformation matrices.

Examples

>>> import rby1_sdk.dynamics as rby_dyn
>>> import numpy as np
>>> # Assume dyn_robot is an initialized rby_dyn.Robot instance
>>> # and dyn_state is a corresponding state object.
>>> dyn_state.set_q(np.random.rand(dyn_robot.get_dof()))
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> # Now you can compute transformations, Jacobians, etc.
>>> transform = dyn_robot.compute_transformation(dyn_state, 0, 1)
>>> print(transform)

compute_gravity_term(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → numpy.ndarray[numpy.float64[18, 1]]#

Computes the gravity compensation term for the robot.

This method calculates the joint torques required to counteract gravity at the current joint positions. The gravity vector must be set in the state object prior to calling this function.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions and the gravity vector.
Returns:: A vector of joint torques required to compensate for gravity.
Return type:: numpy.ndarray

Notes

compute_forward_kinematics must be called before this function.
The gravity vector (spatial acceleration) must be set on the state object using state.set_gravity() or state.set_Vdot0(). For standard gravity along the negative Z-axis, the vector is [0, 0, 0, 0, 0, -9.81].

Examples

>>> # Continuing from a previous example where dyn_robot and dyn_state are set up.
>>> dyn_state.set_gravity(np.array([0, 0, 0, 0, 0, -9.81]))
>>> # or dyn_state.set_Vdot0(np.array([0, 0, 0, 0, 0, 9.81]))  # Note that direction is reversed
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> gravity_torques = dyn_robot.compute_gravity_term(dyn_state)
>>> print(gravity_torques)

compute_inverse_dynamics(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → None#

Computes the inverse dynamics of the robot.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions, velocities, and accelerations. This object will be updated with the computed joint torques. Hello. This is state.

Notes

compute_forward_kinematics, compute_diff_forward_kinematics, and compute_2nd_diff_forward_kinematics must be called in order before this function.

Examples

>>> # This example demonstrates the full sequence for inverse dynamics.
>>> import rby1_sdk as rby
>>> import rby1_sdk.dynamics as rby_dyn
>>> import numpy as np
>>> robot = rby.create_robot_a("localhost:50051")
>>> robot.connect()
>>> dyn_robot = robot.get_dynamics()
>>> dyn_state = dyn_robot.make_state(
...     dyn_robot.get_link_names(), dyn_robot.get_joint_names()
... )
>>> q = (np.random.rand(dyn_robot.get_dof()) - 0.5) * np.pi / 2
>>> dyn_state.set_q(q)
>>> dyn_state.set_qdot(np.zeros(dyn_robot.get_dof()))
>>> dyn_state.set_qddot(np.zeros(dyn_robot.get_dof()))
>>>
>>> # Perform kinematics calculations in order
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> dyn_robot.compute_diff_forward_kinematics(dyn_state)
>>> dyn_robot.compute_2nd_diff_forward_kinematics(dyn_state)
>>>
>>> # Compute inverse dynamics
>>> dyn_robot.compute_inverse_dynamics(dyn_state)
>>>
>>> # Get the resulting torques
>>> torques = dyn_state.get_tau()
>>> with np.printoptions(precision=4, suppress=True):
...     print(f"Inverse dynamics torque (Nm): {torques}")

compute_mass(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, target_link_index: int) → float#

Computes the mass of a specific link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
target_link_index (int) – The index of the target link.

Returns:

The mass of the specified link.

Return type:

compute_mass_matrix(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → numpy.ndarray[numpy.float64[18, 18]]#

Computes the joint space mass matrix (inertia matrix) of the robot.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions.
Returns:: The mass matrix (a square matrix of size DOF x DOF).
Return type:: numpy.ndarray

Notes

compute_forward_kinematics must be called before this function.

compute_mobility_diff_kinematics(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → numpy.ndarray[numpy.float64[3, 1]]#

Computes the forward differential kinematics for the mobile base.

Calculates the linear and angular velocity of the mobile base from the current wheel velocities (qdot).

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including wheel velocities.
Returns:: The resulting body velocity vector [w, vx, vy].
Return type:: numpy.ndarray

compute_mobility_inverse_diff_kinematics(*args, **kwargs)#

Overloaded function.

compute_mobility_inverse_diff_kinematics(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, linear_velocity: numpy.ndarray[numpy.float64[2, 1]], angular_velocity: float) -> None

Computes the inverse differential kinematics for the mobile base.

Calculates the required wheel velocities to achieve a desired linear and angular velocity of the mobile base. Updates the qdot values in the state object.

Parameters:

state (rby1_sdk.dynamics.State) – The robot state object to be updated.
linear_velocity (numpy.ndarray) – The desired linear velocity (x, y) [m/s].
angular_velocity (float) – The desired angular velocity (yaw) [rad/s].

compute_mobility_inverse_diff_kinematics(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, body_velocity: numpy.ndarray[numpy.float64[3, 1]]) -> None

Computes the inverse differential kinematics for the mobile base from a body velocity vector.

Calculates the required wheel velocities from a desired body velocity (twist). Updates the qdot values in the state object.

Parameters:

state (rby1_sdk.dynamics.State) – The robot state object to be updated.
body_velocity (numpy.ndarray) – The desired body velocity vector [w, vx, vy].

compute_reflective_inertia(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 6]]#

Computes the reflective inertia (task space inertia) of the target link with respect to the reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6x6 reflective inertia matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_space_jacobian(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 18]]#

Computes the space Jacobian for a target link relative to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6xDOF space Jacobian matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_total_inertial(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, ref_link: int) → numpy.ndarray[numpy.float64[6, 6]]#

Computes the total spatial inertia of the entire robot with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.

Returns:

The 6x6 total spatial inertia matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_transformation(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[4, 4]]#

Computes the transformation matrix from a reference link to a target link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link (the ‘from’ frame).
target_link_index (int) – The index of the target link (the ‘to’ frame).

Returns:

The 4x4 transformation matrix (SE(3)).

Return type:

Notes

compute_forward_kinematics must be called before this function.

static count_joints(base_link: rby1_sdk.dynamics.Link, include_fixed: bool = False) → int#

count_joints(base_link, include_fixed=False)

Counts the number of joints in a kinematic chain starting from a base link.

Parameters:

base_link (rby1_sdk.dynamics.Link) – The starting link of the kinematic chain.
include_fixed (bool, optional) – Whether to include fixed joints in the count. Default is False.

Returns:

The total number of joints.

Return type:

detect_collisions_or_nearest_links(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, collision_threshold: int = 0) → list[rby1_sdk.dynamics.CollisionResult]#

Detects collisions or finds the nearest links in the robot model.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
collision_threshold (int, optional) – The minimum number of link pairs to return. The function first finds all colliding pairs. If the number of colliding pairs is less than this threshold, it will supplement the result with the nearest non-colliding link pairs until the total count reaches the threshold. The returned list is always sorted by distance. If set to 0, only actual collisions are returned. Default is 0.

Returns:

A list of collision results, sorted by distance.

Return type:

Notes

compute_forward_kinematics must be called before this function.

get_base(self: rby1_sdk.dynamics.Robot_18) → rby1_sdk.dynamics.Link#

Get the base link of the robot.

Returns:: Base link.
Return type:: Link

get_dof(self: rby1_sdk.dynamics.Robot_18) → int#

Get the number of degrees of freedom.

Returns:: Number of degrees of freedom.
Return type:: int

get_joint_names(self: rby1_sdk.dynamics.Robot_18) → list[str]#

Get the list of names of all joints.

Returns:: List of joint names.
Return type:: list

get_joint_property(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, getter: Callable[[rby1_sdk.dynamics.Joint], float]) → numpy.ndarray[numpy.float64[18, 1]]#

Gets a specific property for all joints using a provided getter function.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
getter (callable) – A function that takes a joint object and returns a double value.

Returns:

A vector containing the specified property for each joint.

Return type:

get_limit_q_lower(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → numpy.ndarray[numpy.float64[18, 1]]#

Gets the lower position limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of lower position limits (q) for each joint.
Return type:: numpy.ndarray

get_limit_q_upper(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → numpy.ndarray[numpy.float64[18, 1]]#

Gets the upper position limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of upper position limits (q) for each joint.
Return type:: numpy.ndarray

get_limit_qddot_lower(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → numpy.ndarray[numpy.float64[18, 1]]#

Gets the lower acceleration limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of lower acceleration limits (q_ddot) for each joint.
Return type:: numpy.ndarray

get_limit_qddot_upper(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → numpy.ndarray[numpy.float64[18, 1]]#

Gets the upper acceleration limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of upper acceleration limits (q_ddot) for each joint.
Return type:: numpy.ndarray

get_limit_qdot_lower(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → numpy.ndarray[numpy.float64[18, 1]]#

Gets the lower velocity limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of lower velocity limits (q_dot) for each joint.
Return type:: numpy.ndarray

get_limit_qdot_upper(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → numpy.ndarray[numpy.float64[18, 1]]#

Gets the upper velocity limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of upper velocity limits (q_dot) for each joint.
Return type:: numpy.ndarray

get_limit_torque(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18) → numpy.ndarray[numpy.float64[18, 1]]#

Gets the torque limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of torque limits for each joint.
Return type:: numpy.ndarray

get_link(*args, **kwargs)#

Overloaded function.

get_link(self: rby1_sdk.dynamics.Robot_18, name: str) -> rby1_sdk.dynamics.Link

Get a link by name.

Parameters:: name (str) – Name of the link.
Returns:: Link if found, None otherwise.
Return type:: Link, optional

get_link(self: rby1_sdk.dynamics.Robot_18, state: rby1_sdk.dynamics.State_18, index: int) -> rby1_sdk.dynamics.Link

Get a link by state and index.

Parameters:

state (State) – Current state of the robot.
index (int) – Index of the link.

Returns:

Link if found, None otherwise.

Return type:

Link, optional

get_link_names(self: rby1_sdk.dynamics.Robot_18) → list[str]#

Get the list of names of all links.

Returns:: List of link names.
Return type:: list

get_number_of_joints(self: rby1_sdk.dynamics.Robot_18) → int#

Get the number of joints.

Returns:: Number of joints.
Return type:: int

make_state(self: rby1_sdk.dynamics.Robot_18, link_names: list[str], joint_names: list[str]) → rby1_sdk.dynamics.State_18#

Create a state from link and joint names.

Parameters:

link_names (list[str]) – List of link names.
joint_names (list[str]) – List of joint names.

Returns:

A new state object.

Return type:

Examples

>>> import rby1_sdk.dynamics as rby_dyn
>>> import numpy as np
>>>
>>> link_0 = rby_dyn.Link("link_0")
>>> link_1 = rby_dyn.Link("link_1")
>>>
>>> joint_0 = rby_dyn.Joint.make_revolute("joint_0", np.identity(4), np.array([0, 0, 1]))
>>> joint_0.connect_links(link_0, link_1, np.identity(4), np.identity(4))
>>>
>>> dyn_robot = rby_dyn.Robot(
...     rby_dyn.RobotConfiguration(name="sample_robot", base_link=link_0)
... )
>>>
>>> dyn_state = dyn_robot.make_state(["link_0", "link_1"], ["joint_0"])
>>> dyn_state.set_q(np.array([np.pi / 2]))  # Angle of joint_0 is 90 degrees
>>>
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> # Calculate transformation from link_0 to link_1
>>> transform = dyn_robot.compute_transformation(dyn_state, 0, 1)  # 0: link_0, 1: link_1
>>> print(transform)

class Robot_24#

Bases: pybind11_object

Robot (DOF=24) dynamics model.

Represents the dynamics of a robot with a given number of degrees of freedom.

base#

Base link of the robot.

Type:: Link

link_names#

List of names of all links.

Type:: list

joint_names#

List of names of all joints.

Type:: list

compute_2nd_diff_forward_kinematics(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → None#

Computes the second-order differential forward kinematics for each joint.

This method calculates the body acceleration for each joint frame based on the current joint accelerations (qddot) in the state. The results are cached within the state object.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions, velocities, and accelerations. This object will be updated with the computed body accelerations.

Notes

compute_forward_kinematics and compute_diff_forward_kinematics must be called before this function.

Examples

>>> # Continuing from the previous example...
>>> dyn_state.set_qddot(np.zeros(dyn_robot.get_dof()))
>>> dyn_robot.compute_2nd_diff_forward_kinematics(dyn_state)

compute_body_jacobian(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 24]]#

Computes the body Jacobian for a target link relative to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6xDOF body Jacobian matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_body_velocity(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 1]]#

Computes the relative body velocity (twist) of a target link with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6x1 body velocity vector (twist).

Return type:

Notes

compute_forward_kinematics and compute_diff_forward_kinematics must be called before this function.

compute_center_of_mass(*args, **kwargs)#

Overloaded function.

compute_center_of_mass(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, ref_link: int, target_link: int) -> numpy.ndarray[numpy.float64[3, 1]]

Computes the center of mass of a single target link with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.
target_link (int) – The index of the target link.

Returns:

The 3D position vector of the center of mass.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, ref_link: int, target_links: list[int]) -> numpy.ndarray[numpy.float64[3, 1]]

Computes the combined center of mass of multiple target links with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.
target_links (list[int]) – A list of indices of the target links.

Returns:

The 3D position vector of the combined center of mass.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, ref_link: int) -> numpy.ndarray[numpy.float64[3, 1]]

Computes the center of mass of the entire robot with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.

Returns:

The 3D position vector of the total center of mass.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass_jacobian(*args, **kwargs)#

Overloaded function.

compute_center_of_mass_jacobian(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, ref_link: int, target_link: int) -> numpy.ndarray[numpy.float64[3, 24]]

Computes the Jacobian for the center of mass of a single target link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.
target_link (int) – The index of the target link.

Returns:

The 3xDOF center of mass Jacobian matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass_jacobian(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, ref_link: int) -> numpy.ndarray[numpy.float64[3, 24]]

Computes the Jacobian for the center of mass of the entire robot.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.

Returns:

The 3xDOF center of mass Jacobian matrix for the whole robot.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_diff_forward_kinematics(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → None#

Computes the differential forward kinematics for each joint.

This method calculates the body velocity (twist) for each joint frame based on the current joint velocities (qdot) in the state. The results are cached within the state object.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, which includes joint velocities. This object will be updated with the computed body velocities.

Notes

compute_forward_kinematics must be called before this function.

Examples

>>> # Continuing from the previous example...
>>> dyn_state.set_qdot(np.zeros(dyn_robot.get_dof()))
>>> dyn_robot.compute_diff_forward_kinematics(dyn_state)

compute_forward_kinematics(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → None#

Computes the forward kinematics for each joint.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, which includes joint positions. This object will be updated with the computed transformation matrices.

Examples

>>> import rby1_sdk.dynamics as rby_dyn
>>> import numpy as np
>>> # Assume dyn_robot is an initialized rby_dyn.Robot instance
>>> # and dyn_state is a corresponding state object.
>>> dyn_state.set_q(np.random.rand(dyn_robot.get_dof()))
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> # Now you can compute transformations, Jacobians, etc.
>>> transform = dyn_robot.compute_transformation(dyn_state, 0, 1)
>>> print(transform)

compute_gravity_term(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → numpy.ndarray[numpy.float64[24, 1]]#

Computes the gravity compensation term for the robot.

This method calculates the joint torques required to counteract gravity at the current joint positions. The gravity vector must be set in the state object prior to calling this function.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions and the gravity vector.
Returns:: A vector of joint torques required to compensate for gravity.
Return type:: numpy.ndarray

Notes

compute_forward_kinematics must be called before this function.
The gravity vector (spatial acceleration) must be set on the state object using state.set_gravity() or state.set_Vdot0(). For standard gravity along the negative Z-axis, the vector is [0, 0, 0, 0, 0, -9.81].

Examples

>>> # Continuing from a previous example where dyn_robot and dyn_state are set up.
>>> dyn_state.set_gravity(np.array([0, 0, 0, 0, 0, -9.81]))
>>> # or dyn_state.set_Vdot0(np.array([0, 0, 0, 0, 0, 9.81]))  # Note that direction is reversed
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> gravity_torques = dyn_robot.compute_gravity_term(dyn_state)
>>> print(gravity_torques)

compute_inverse_dynamics(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → None#

Computes the inverse dynamics of the robot.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions, velocities, and accelerations. This object will be updated with the computed joint torques. Hello. This is state.

Notes

compute_forward_kinematics, compute_diff_forward_kinematics, and compute_2nd_diff_forward_kinematics must be called in order before this function.

Examples

>>> # This example demonstrates the full sequence for inverse dynamics.
>>> import rby1_sdk as rby
>>> import rby1_sdk.dynamics as rby_dyn
>>> import numpy as np
>>> robot = rby.create_robot_a("localhost:50051")
>>> robot.connect()
>>> dyn_robot = robot.get_dynamics()
>>> dyn_state = dyn_robot.make_state(
...     dyn_robot.get_link_names(), dyn_robot.get_joint_names()
... )
>>> q = (np.random.rand(dyn_robot.get_dof()) - 0.5) * np.pi / 2
>>> dyn_state.set_q(q)
>>> dyn_state.set_qdot(np.zeros(dyn_robot.get_dof()))
>>> dyn_state.set_qddot(np.zeros(dyn_robot.get_dof()))
>>>
>>> # Perform kinematics calculations in order
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> dyn_robot.compute_diff_forward_kinematics(dyn_state)
>>> dyn_robot.compute_2nd_diff_forward_kinematics(dyn_state)
>>>
>>> # Compute inverse dynamics
>>> dyn_robot.compute_inverse_dynamics(dyn_state)
>>>
>>> # Get the resulting torques
>>> torques = dyn_state.get_tau()
>>> with np.printoptions(precision=4, suppress=True):
...     print(f"Inverse dynamics torque (Nm): {torques}")

compute_mass(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, target_link_index: int) → float#

Computes the mass of a specific link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
target_link_index (int) – The index of the target link.

Returns:

The mass of the specified link.

Return type:

compute_mass_matrix(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → numpy.ndarray[numpy.float64[24, 24]]#

Computes the joint space mass matrix (inertia matrix) of the robot.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions.
Returns:: The mass matrix (a square matrix of size DOF x DOF).
Return type:: numpy.ndarray

Notes

compute_forward_kinematics must be called before this function.

compute_mobility_diff_kinematics(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → numpy.ndarray[numpy.float64[3, 1]]#

Computes the forward differential kinematics for the mobile base.

Calculates the linear and angular velocity of the mobile base from the current wheel velocities (qdot).

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including wheel velocities.
Returns:: The resulting body velocity vector [w, vx, vy].
Return type:: numpy.ndarray

compute_mobility_inverse_diff_kinematics(*args, **kwargs)#

Overloaded function.

compute_mobility_inverse_diff_kinematics(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, linear_velocity: numpy.ndarray[numpy.float64[2, 1]], angular_velocity: float) -> None

Computes the inverse differential kinematics for the mobile base.

Calculates the required wheel velocities to achieve a desired linear and angular velocity of the mobile base. Updates the qdot values in the state object.

Parameters:

state (rby1_sdk.dynamics.State) – The robot state object to be updated.
linear_velocity (numpy.ndarray) – The desired linear velocity (x, y) [m/s].
angular_velocity (float) – The desired angular velocity (yaw) [rad/s].

compute_mobility_inverse_diff_kinematics(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, body_velocity: numpy.ndarray[numpy.float64[3, 1]]) -> None

Computes the inverse differential kinematics for the mobile base from a body velocity vector.

Calculates the required wheel velocities from a desired body velocity (twist). Updates the qdot values in the state object.

Parameters:

state (rby1_sdk.dynamics.State) – The robot state object to be updated.
body_velocity (numpy.ndarray) – The desired body velocity vector [w, vx, vy].

compute_reflective_inertia(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 6]]#

Computes the reflective inertia (task space inertia) of the target link with respect to the reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6x6 reflective inertia matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_space_jacobian(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 24]]#

Computes the space Jacobian for a target link relative to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6xDOF space Jacobian matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_total_inertial(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, ref_link: int) → numpy.ndarray[numpy.float64[6, 6]]#

Computes the total spatial inertia of the entire robot with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.

Returns:

The 6x6 total spatial inertia matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_transformation(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[4, 4]]#

Computes the transformation matrix from a reference link to a target link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link (the ‘from’ frame).
target_link_index (int) – The index of the target link (the ‘to’ frame).

Returns:

The 4x4 transformation matrix (SE(3)).

Return type:

Notes

compute_forward_kinematics must be called before this function.

static count_joints(base_link: rby1_sdk.dynamics.Link, include_fixed: bool = False) → int#

count_joints(base_link, include_fixed=False)

Counts the number of joints in a kinematic chain starting from a base link.

Parameters:

base_link (rby1_sdk.dynamics.Link) – The starting link of the kinematic chain.
include_fixed (bool, optional) – Whether to include fixed joints in the count. Default is False.

Returns:

The total number of joints.

Return type:

detect_collisions_or_nearest_links(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, collision_threshold: int = 0) → list[rby1_sdk.dynamics.CollisionResult]#

Detects collisions or finds the nearest links in the robot model.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
collision_threshold (int, optional) – The minimum number of link pairs to return. The function first finds all colliding pairs. If the number of colliding pairs is less than this threshold, it will supplement the result with the nearest non-colliding link pairs until the total count reaches the threshold. The returned list is always sorted by distance. If set to 0, only actual collisions are returned. Default is 0.

Returns:

A list of collision results, sorted by distance.

Return type:

Notes

compute_forward_kinematics must be called before this function.

get_base(self: rby1_sdk.dynamics.Robot_24) → rby1_sdk.dynamics.Link#

Get the base link of the robot.

Returns:: Base link.
Return type:: Link

get_dof(self: rby1_sdk.dynamics.Robot_24) → int#

Get the number of degrees of freedom.

Returns:: Number of degrees of freedom.
Return type:: int

get_joint_names(self: rby1_sdk.dynamics.Robot_24) → list[str]#

Get the list of names of all joints.

Returns:: List of joint names.
Return type:: list

get_joint_property(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, getter: Callable[[rby1_sdk.dynamics.Joint], float]) → numpy.ndarray[numpy.float64[24, 1]]#

Gets a specific property for all joints using a provided getter function.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
getter (callable) – A function that takes a joint object and returns a double value.

Returns:

A vector containing the specified property for each joint.

Return type:

get_limit_q_lower(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → numpy.ndarray[numpy.float64[24, 1]]#

Gets the lower position limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of lower position limits (q) for each joint.
Return type:: numpy.ndarray

get_limit_q_upper(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → numpy.ndarray[numpy.float64[24, 1]]#

Gets the upper position limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of upper position limits (q) for each joint.
Return type:: numpy.ndarray

get_limit_qddot_lower(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → numpy.ndarray[numpy.float64[24, 1]]#

Gets the lower acceleration limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of lower acceleration limits (q_ddot) for each joint.
Return type:: numpy.ndarray

get_limit_qddot_upper(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → numpy.ndarray[numpy.float64[24, 1]]#

Gets the upper acceleration limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of upper acceleration limits (q_ddot) for each joint.
Return type:: numpy.ndarray

get_limit_qdot_lower(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → numpy.ndarray[numpy.float64[24, 1]]#

Gets the lower velocity limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of lower velocity limits (q_dot) for each joint.
Return type:: numpy.ndarray

get_limit_qdot_upper(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → numpy.ndarray[numpy.float64[24, 1]]#

Gets the upper velocity limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of upper velocity limits (q_dot) for each joint.
Return type:: numpy.ndarray

get_limit_torque(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24) → numpy.ndarray[numpy.float64[24, 1]]#

Gets the torque limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of torque limits for each joint.
Return type:: numpy.ndarray

get_link(*args, **kwargs)#

Overloaded function.

get_link(self: rby1_sdk.dynamics.Robot_24, name: str) -> rby1_sdk.dynamics.Link

Get a link by name.

Parameters:: name (str) – Name of the link.
Returns:: Link if found, None otherwise.
Return type:: Link, optional

get_link(self: rby1_sdk.dynamics.Robot_24, state: rby1_sdk.dynamics.State_24, index: int) -> rby1_sdk.dynamics.Link

Get a link by state and index.

Parameters:

state (State) – Current state of the robot.
index (int) – Index of the link.

Returns:

Link if found, None otherwise.

Return type:

Link, optional

get_link_names(self: rby1_sdk.dynamics.Robot_24) → list[str]#

Get the list of names of all links.

Returns:: List of link names.
Return type:: list

get_number_of_joints(self: rby1_sdk.dynamics.Robot_24) → int#

Get the number of joints.

Returns:: Number of joints.
Return type:: int

make_state(self: rby1_sdk.dynamics.Robot_24, link_names: list[str], joint_names: list[str]) → rby1_sdk.dynamics.State_24#

Create a state from link and joint names.

Parameters:

link_names (list[str]) – List of link names.
joint_names (list[str]) – List of joint names.

Returns:

A new state object.

Return type:

Examples

>>> import rby1_sdk.dynamics as rby_dyn
>>> import numpy as np
>>>
>>> link_0 = rby_dyn.Link("link_0")
>>> link_1 = rby_dyn.Link("link_1")
>>>
>>> joint_0 = rby_dyn.Joint.make_revolute("joint_0", np.identity(4), np.array([0, 0, 1]))
>>> joint_0.connect_links(link_0, link_1, np.identity(4), np.identity(4))
>>>
>>> dyn_robot = rby_dyn.Robot(
...     rby_dyn.RobotConfiguration(name="sample_robot", base_link=link_0)
... )
>>>
>>> dyn_state = dyn_robot.make_state(["link_0", "link_1"], ["joint_0"])
>>> dyn_state.set_q(np.array([np.pi / 2]))  # Angle of joint_0 is 90 degrees
>>>
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> # Calculate transformation from link_0 to link_1
>>> transform = dyn_robot.compute_transformation(dyn_state, 0, 1)  # 0: link_0, 1: link_1
>>> print(transform)

class Robot_26#

Bases: pybind11_object

Robot (DOF=26) dynamics model.

Represents the dynamics of a robot with a given number of degrees of freedom.

base#

Base link of the robot.

Type:: Link

link_names#

List of names of all links.

Type:: list

joint_names#

List of names of all joints.

Type:: list

compute_2nd_diff_forward_kinematics(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → None#

Computes the second-order differential forward kinematics for each joint.

This method calculates the body acceleration for each joint frame based on the current joint accelerations (qddot) in the state. The results are cached within the state object.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions, velocities, and accelerations. This object will be updated with the computed body accelerations.

Notes

compute_forward_kinematics and compute_diff_forward_kinematics must be called before this function.

Examples

>>> # Continuing from the previous example...
>>> dyn_state.set_qddot(np.zeros(dyn_robot.get_dof()))
>>> dyn_robot.compute_2nd_diff_forward_kinematics(dyn_state)

compute_body_jacobian(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 26]]#

Computes the body Jacobian for a target link relative to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6xDOF body Jacobian matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_body_velocity(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 1]]#

Computes the relative body velocity (twist) of a target link with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6x1 body velocity vector (twist).

Return type:

Notes

compute_forward_kinematics and compute_diff_forward_kinematics must be called before this function.

compute_center_of_mass(*args, **kwargs)#

Overloaded function.

compute_center_of_mass(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, ref_link: int, target_link: int) -> numpy.ndarray[numpy.float64[3, 1]]

Computes the center of mass of a single target link with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.
target_link (int) – The index of the target link.

Returns:

The 3D position vector of the center of mass.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, ref_link: int, target_links: list[int]) -> numpy.ndarray[numpy.float64[3, 1]]

Computes the combined center of mass of multiple target links with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.
target_links (list[int]) – A list of indices of the target links.

Returns:

The 3D position vector of the combined center of mass.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, ref_link: int) -> numpy.ndarray[numpy.float64[3, 1]]

Computes the center of mass of the entire robot with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.

Returns:

The 3D position vector of the total center of mass.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass_jacobian(*args, **kwargs)#

Overloaded function.

compute_center_of_mass_jacobian(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, ref_link: int, target_link: int) -> numpy.ndarray[numpy.float64[3, 26]]

Computes the Jacobian for the center of mass of a single target link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.
target_link (int) – The index of the target link.

Returns:

The 3xDOF center of mass Jacobian matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_center_of_mass_jacobian(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, ref_link: int) -> numpy.ndarray[numpy.float64[3, 26]]

Computes the Jacobian for the center of mass of the entire robot.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.

Returns:

The 3xDOF center of mass Jacobian matrix for the whole robot.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_diff_forward_kinematics(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → None#

Computes the differential forward kinematics for each joint.

This method calculates the body velocity (twist) for each joint frame based on the current joint velocities (qdot) in the state. The results are cached within the state object.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, which includes joint velocities. This object will be updated with the computed body velocities.

Notes

compute_forward_kinematics must be called before this function.

Examples

>>> # Continuing from the previous example...
>>> dyn_state.set_qdot(np.zeros(dyn_robot.get_dof()))
>>> dyn_robot.compute_diff_forward_kinematics(dyn_state)

compute_forward_kinematics(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → None#

Computes the forward kinematics for each joint.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, which includes joint positions. This object will be updated with the computed transformation matrices.

Examples

>>> import rby1_sdk.dynamics as rby_dyn
>>> import numpy as np
>>> # Assume dyn_robot is an initialized rby_dyn.Robot instance
>>> # and dyn_state is a corresponding state object.
>>> dyn_state.set_q(np.random.rand(dyn_robot.get_dof()))
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> # Now you can compute transformations, Jacobians, etc.
>>> transform = dyn_robot.compute_transformation(dyn_state, 0, 1)
>>> print(transform)

compute_gravity_term(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → numpy.ndarray[numpy.float64[26, 1]]#

Computes the gravity compensation term for the robot.

This method calculates the joint torques required to counteract gravity at the current joint positions. The gravity vector must be set in the state object prior to calling this function.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions and the gravity vector.
Returns:: A vector of joint torques required to compensate for gravity.
Return type:: numpy.ndarray

Notes

compute_forward_kinematics must be called before this function.
The gravity vector (spatial acceleration) must be set on the state object using state.set_gravity() or state.set_Vdot0(). For standard gravity along the negative Z-axis, the vector is [0, 0, 0, 0, 0, -9.81].

Examples

>>> # Continuing from a previous example where dyn_robot and dyn_state are set up.
>>> dyn_state.set_gravity(np.array([0, 0, 0, 0, 0, -9.81]))
>>> # or dyn_state.set_Vdot0(np.array([0, 0, 0, 0, 0, 9.81]))  # Note that direction is reversed
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> gravity_torques = dyn_robot.compute_gravity_term(dyn_state)
>>> print(gravity_torques)

compute_inverse_dynamics(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → None#

Computes the inverse dynamics of the robot.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions, velocities, and accelerations. This object will be updated with the computed joint torques. Hello. This is state.

Notes

compute_forward_kinematics, compute_diff_forward_kinematics, and compute_2nd_diff_forward_kinematics must be called in order before this function.

Examples

>>> # This example demonstrates the full sequence for inverse dynamics.
>>> import rby1_sdk as rby
>>> import rby1_sdk.dynamics as rby_dyn
>>> import numpy as np
>>> robot = rby.create_robot_a("localhost:50051")
>>> robot.connect()
>>> dyn_robot = robot.get_dynamics()
>>> dyn_state = dyn_robot.make_state(
...     dyn_robot.get_link_names(), dyn_robot.get_joint_names()
... )
>>> q = (np.random.rand(dyn_robot.get_dof()) - 0.5) * np.pi / 2
>>> dyn_state.set_q(q)
>>> dyn_state.set_qdot(np.zeros(dyn_robot.get_dof()))
>>> dyn_state.set_qddot(np.zeros(dyn_robot.get_dof()))
>>>
>>> # Perform kinematics calculations in order
>>> dyn_robot.compute_forward_kinematics(dyn_state)
>>> dyn_robot.compute_diff_forward_kinematics(dyn_state)
>>> dyn_robot.compute_2nd_diff_forward_kinematics(dyn_state)
>>>
>>> # Compute inverse dynamics
>>> dyn_robot.compute_inverse_dynamics(dyn_state)
>>>
>>> # Get the resulting torques
>>> torques = dyn_state.get_tau()
>>> with np.printoptions(precision=4, suppress=True):
...     print(f"Inverse dynamics torque (Nm): {torques}")

compute_mass(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, target_link_index: int) → float#

Computes the mass of a specific link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
target_link_index (int) – The index of the target link.

Returns:

The mass of the specified link.

Return type:

compute_mass_matrix(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → numpy.ndarray[numpy.float64[26, 26]]#

Computes the joint space mass matrix (inertia matrix) of the robot.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including joint positions.
Returns:: The mass matrix (a square matrix of size DOF x DOF).
Return type:: numpy.ndarray

Notes

compute_forward_kinematics must be called before this function.

compute_mobility_diff_kinematics(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → numpy.ndarray[numpy.float64[3, 1]]#

Computes the forward differential kinematics for the mobile base.

Calculates the linear and angular velocity of the mobile base from the current wheel velocities (qdot).

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot, including wheel velocities.
Returns:: The resulting body velocity vector [w, vx, vy].
Return type:: numpy.ndarray

compute_mobility_inverse_diff_kinematics(*args, **kwargs)#

Overloaded function.

compute_mobility_inverse_diff_kinematics(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, linear_velocity: numpy.ndarray[numpy.float64[2, 1]], angular_velocity: float) -> None

Computes the inverse differential kinematics for the mobile base.

Calculates the required wheel velocities to achieve a desired linear and angular velocity of the mobile base. Updates the qdot values in the state object.

Parameters:

state (rby1_sdk.dynamics.State) – The robot state object to be updated.
linear_velocity (numpy.ndarray) – The desired linear velocity (x, y) [m/s].
angular_velocity (float) – The desired angular velocity (yaw) [rad/s].

compute_mobility_inverse_diff_kinematics(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, body_velocity: numpy.ndarray[numpy.float64[3, 1]]) -> None

Computes the inverse differential kinematics for the mobile base from a body velocity vector.

Calculates the required wheel velocities from a desired body velocity (twist). Updates the qdot values in the state object.

Parameters:

state (rby1_sdk.dynamics.State) – The robot state object to be updated.
body_velocity (numpy.ndarray) – The desired body velocity vector [w, vx, vy].

compute_reflective_inertia(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 6]]#

Computes the reflective inertia (task space inertia) of the target link with respect to the reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6x6 reflective inertia matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_space_jacobian(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[6, 26]]#

Computes the space Jacobian for a target link relative to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link.
target_link_index (int) – The index of the target link.

Returns:

The 6xDOF space Jacobian matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_total_inertial(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, ref_link: int) → numpy.ndarray[numpy.float64[6, 6]]#

Computes the total spatial inertia of the entire robot with respect to a reference link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
ref_link (int) – The index of the reference link frame.

Returns:

The 6x6 total spatial inertia matrix.

Return type:

Notes

compute_forward_kinematics must be called before this function.

compute_transformation(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, reference_link_index: int, target_link_index: int) → numpy.ndarray[numpy.float64[4, 4]]#

Computes the transformation matrix from a reference link to a target link.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
reference_link_index (int) – The index of the reference link (the ‘from’ frame).
target_link_index (int) – The index of the target link (the ‘to’ frame).

Returns:

The 4x4 transformation matrix (SE(3)).

Return type:

Notes

compute_forward_kinematics must be called before this function.

static count_joints(base_link: rby1_sdk.dynamics.Link, include_fixed: bool = False) → int#

count_joints(base_link, include_fixed=False)

Counts the number of joints in a kinematic chain starting from a base link.

Parameters:

base_link (rby1_sdk.dynamics.Link) – The starting link of the kinematic chain.
include_fixed (bool, optional) – Whether to include fixed joints in the count. Default is False.

Returns:

The total number of joints.

Return type:

detect_collisions_or_nearest_links(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, collision_threshold: int = 0) → list[rby1_sdk.dynamics.CollisionResult]#

Detects collisions or finds the nearest links in the robot model.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
collision_threshold (int, optional) – The minimum number of link pairs to return. The function first finds all colliding pairs. If the number of colliding pairs is less than this threshold, it will supplement the result with the nearest non-colliding link pairs until the total count reaches the threshold. The returned list is always sorted by distance. If set to 0, only actual collisions are returned. Default is 0.

Returns:

A list of collision results, sorted by distance.

Return type:

Notes

compute_forward_kinematics must be called before this function.

get_base(self: rby1_sdk.dynamics.Robot_26) → rby1_sdk.dynamics.Link#

Get the base link of the robot.

Returns:: Base link.
Return type:: Link

get_dof(self: rby1_sdk.dynamics.Robot_26) → int#

Get the number of degrees of freedom.

Returns:: Number of degrees of freedom.
Return type:: int

get_joint_names(self: rby1_sdk.dynamics.Robot_26) → list[str]#

Get the list of names of all joints.

Returns:: List of joint names.
Return type:: list

get_joint_property(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, getter: Callable[[rby1_sdk.dynamics.Joint], float]) → numpy.ndarray[numpy.float64[26, 1]]#

Gets a specific property for all joints using a provided getter function.

Parameters:

state (rby1_sdk.dynamics.State) – The current state of the robot.
getter (callable) – A function that takes a joint object and returns a double value.

Returns:

A vector containing the specified property for each joint.

Return type:

get_limit_q_lower(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → numpy.ndarray[numpy.float64[26, 1]]#

Gets the lower position limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of lower position limits (q) for each joint.
Return type:: numpy.ndarray

get_limit_q_upper(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → numpy.ndarray[numpy.float64[26, 1]]#

Gets the upper position limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of upper position limits (q) for each joint.
Return type:: numpy.ndarray

get_limit_qddot_lower(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → numpy.ndarray[numpy.float64[26, 1]]#

Gets the lower acceleration limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of lower acceleration limits (q_ddot) for each joint.
Return type:: numpy.ndarray

get_limit_qddot_upper(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → numpy.ndarray[numpy.float64[26, 1]]#

Gets the upper acceleration limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of upper acceleration limits (q_ddot) for each joint.
Return type:: numpy.ndarray

get_limit_qdot_lower(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → numpy.ndarray[numpy.float64[26, 1]]#

Gets the lower velocity limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of lower velocity limits (q_dot) for each joint.
Return type:: numpy.ndarray

get_limit_qdot_upper(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → numpy.ndarray[numpy.float64[26, 1]]#

Gets the upper velocity limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of upper velocity limits (q_dot) for each joint.
Return type:: numpy.ndarray

get_limit_torque(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26) → numpy.ndarray[numpy.float64[26, 1]]#

Gets the torque limits for all joints.

Parameters:: state (rby1_sdk.dynamics.State) – The current state of the robot.
Returns:: A vector of torque limits for each joint.
Return type:: numpy.ndarray

get_link(*args, **kwargs)#

Overloaded function.

get_link(self: rby1_sdk.dynamics.Robot_26, name: str) -> rby1_sdk.dynamics.Link

Get a link by name.

Parameters:: name (str) – Name of the link.
Returns:: Link if found, None otherwise.
Return type:: Link, optional

get_link(self: rby1_sdk.dynamics.Robot_26, state: rby1_sdk.dynamics.State_26, index: int) -> rby1_sdk.dynamics.Link

Get a link by state and index.

Parameters:

state (State) – Current state of the robot.
index (int) – Index of the link.

Returns:

Link if found, None otherwise.

Return type:

Link, optional

get_link_names(self: rby1_sdk.dynamics.Robot_26) → list[str]#

Get the list of names of all links.

Returns:: List of link names.
Return type:: list

get_number_of_joints(self: rby1_sdk.dynamics.Robot_26) → int#

Get the number of joints.

Returns:: Number of joints.
Return type:: int

make_state(self: rby1_sdk.dynamics.Robot_26, link_names: list[str], joint_names: list[str]) → rby1_sdk.dynamics.State_26#

Create a state from link and joint names.

Parameters:

link_names (list[str]) – List of link names.
joint_names (list[str]) – List of joint names.

Returns:

A new state object.

Return type: