Create a solver

Create a solver#

Overview#

The solver module in EmbodiChain provides a unified and extensible interface for robot kinematics computation, including forward kinematics (FK), inverse kinematics (IK), and Jacobian calculation. It supports multiple solver backends (e.g., Pinocchio, OPW, SRS, PINK, PyTorch) and is designed for both simulation and real-robot applications.

Key Features#

Unified API: Abstract base class (BaseSolver) defines a common interface for all solvers.
Multiple Backends: Supports Pinocchio, OPW, SRS, PINK, PyTorch, and differential solvers.
Flexible Configuration: Easily switch solver type and parameters via configuration.
Batch and Single Query: Supports both batch and single FK/IK/Jacobian queries.
Extensible: New solvers can be added by subclassing BaseSolver and implementing required methods.

The Code#

The tutorial corresponds to the srs_solver.py script in the scripts/tutorials/sim directory.

Code for srs_solver.py

# ----------------------------------------------------------------------------
# Copyright (c) 2021-2025 DexForce Technology Co., Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ----------------------------------------------------------------------------

import time
import numpy as np
import torch

from IPython import embed

from embodichain.lab.sim.objects import Robot
from embodichain.lab.sim import SimulationManager, SimulationManagerCfg
from embodichain.lab.sim.robots import DexforceW1Cfg


def main():
    # Set print options for better readability
    np.set_printoptions(precision=5, suppress=True)
    torch.set_printoptions(precision=5, sci_mode=False)

    # Initialize simulation
    sim_device = "cpu"
    sim = SimulationManager(
        SimulationManagerCfg(
            headless=False, sim_device=sim_device, width=2200, height=1200
        )
    )

    sim.build_multiple_arenas(1)
    sim.set_manual_update(False)

    robot: Robot = sim.add_robot(cfg=DexforceW1Cfg.from_dict({"uid": "dexforce_w1"}))
    arm_name = "left_arm"
    # Set initial joint positions for left arm
    qpos_fk_list = [
        torch.tensor([[0.0, 0.0, 0.0, -np.pi / 2, 0.0, 0.0, 0.0]], dtype=torch.float32),
    ]
    robot.set_qpos(qpos_fk_list[0], joint_ids=robot.get_joint_ids(arm_name))

    time.sleep(0.5)

    fk_xpos_batch = torch.cat(qpos_fk_list, dim=0)

    fk_xpos_list = robot.compute_fk(qpos=fk_xpos_batch, name=arm_name, to_matrix=True)

    start_time = time.time()
    res, ik_qpos = robot.compute_ik(
        pose=fk_xpos_list,
        name=arm_name,
        # joint_seed=qpos_fk_list[0],
        return_all_solutions=True,
    )
    end_time = time.time()
    print(
        f"Batch IK computation time for {len(fk_xpos_list)} poses: {end_time - start_time:.6f} seconds"
    )

    if ik_qpos.dim() == 3:
        first_solutions = ik_qpos[:, 0, :]
    else:
        first_solutions = ik_qpos
    robot.set_qpos(first_solutions, joint_ids=robot.get_joint_ids(arm_name))

    ik_xpos_list = robot.compute_fk(qpos=first_solutions, name=arm_name, to_matrix=True)

    print("fk_xpos_list: ", fk_xpos_list)
    print("ik_xpos_list: ", ik_xpos_list)

    embed(header="Test SRSSolver example. Press Ctrl-D to exit.")


if __name__ == "__main__":
    main()

Typical Usage#

Step 1: Configure solver

srs_cfg = SrsSolverCfg(
    urdf_path="/path/to/robot.urdf",
    joint_names=[
        "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
        "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
    ],
    end_link_name="ee_link",
    root_link_name="base_link"
)

Step 2: Instantiate the robot with solver

robot_cfg.solver_cfg = srs_cfg
robot = Robot(cfg=robot_cfg, entities=[], device="cpu")

Step 3: Use FK/IK/Jacobian

qpos = [0.0, -1.57, 1.57, 0.0, 1.57, 0.0]
ee_pose = robot.compute_fk(qpos)

target_pose = np.array([
    [0, -1, 0, 0.5],
    [1,  0, 0, 0.2],
    [0,  0, 1, 0.3],
    [0,  0, 0, 1.0]
])
success, qpos_sol = robot.compute_ik(target_pose, joint_seed=qpos)
J = robot.get_solver().get_jacobian(qpos)

Note

robot.compute_fk(qpos) internally calls the bound solver’s get_fk method.
robot.compute_ik(target_pose, joint_seed) internally calls the solver’s get_ik method.

API Reference#

BaseSolver

class BaseSolver:
    def get_fk(self, qpos, **kwargs) -> torch.Tensor:
        """Compute forward kinematics for the end-effector."""

    def get_ik(self, target_pose, joint_seed=None, num_samples=None, **kwargs) -> Tuple[torch.Tensor, torch.Tensor]:
        """Compute inverse kinematics for a given pose."""

    def get_jacobian(self, qpos, locations=None, jac_type="full") -> torch.Tensor:
        """Compute the Jacobian matrix for the given joint positions."""

set_ik_nearst_weight: Set weights for IK nearest neighbor search.
set_position_limits / get_position_limits: Set or get joint position limits.
set_tcp / get_tcp: Set or get the tool center point (TCP) transformation.

Configuration#

All solvers are configured via a SolverCfg or its subclass (e.g., PinkSolverCfg).
Key config fields: urdf_path, joint_names, end_link_name, root_link_name, tcp, and solver-specific parameters.
Use cfg.init_solver() to instantiate the solver, or assign to robot_cfg.solver_cfg for automatic integration.

Notes & Best Practices#

Always ensure URDF and joint/link names match your robot model.
For IK, providing a good qpos_seed improves convergence and solution quality.
Use set_iteration_params (if available) to tune solver performance for your application.
For custom robots or new algorithms, subclass BaseSolver and register your solver.