From bf2589a10a61d1dc637d0da8591081bbd4c88886 Mon Sep 17 00:00:00 2001 From: chenjian Date: Wed, 11 Mar 2026 16:46:40 +0800 Subject: [PATCH 1/9] update --- embodichain/lab/sim/planners/base_planner.py | 11 ++++--- .../lab/sim/planners/motion_generator.py | 22 +++++++++++-- .../lab/sim/planners/toppra_planner.py | 26 +++++++-------- embodichain/lab/sim/planners/utils.py | 32 +++++++++++++++++++ 4 files changed, 69 insertions(+), 22 deletions(-) diff --git a/embodichain/lab/sim/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index 5ace6c6d..0a5d8ebb 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -21,6 +21,7 @@ from embodichain.lab.sim.planners.utils import TrajectorySampleMethod from embodichain.utils import logger +from embodichain.lab.sim.planners.utils import PlanState class BasePlanner(ABC): @@ -34,15 +35,15 @@ class BasePlanner(ABC): max_constraints: Dictionary containing 'velocity' and 'acceleration' constraints """ - def __init__(self, dofs: int, max_constraints: Dict[str, List[float]]): - self.dofs = dofs - self.max_constraints = max_constraints + def __init__(self, **kwargs): + self.dofs = kwargs.get("dofs", None) + self.max_constraints = kwargs.get("max_constraints", None) @abstractmethod def plan( self, - current_state: Dict, - target_states: List[Dict], + current_state: PlanState, + target_states: list[PlanState], **kwargs, ) -> Tuple[ bool, diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index 156792cf..eb36aec5 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -24,6 +24,7 @@ from embodichain.lab.sim.planners.utils import TrajectorySampleMethod from embodichain.lab.sim.objects.robot import Robot from embodichain.utils import logger +from embodichain.lab.sim.planners.utils import PlanState, MoveType, MovePart class PlannerType(Enum): @@ -128,7 +129,7 @@ def _create_planner( ) if planner_type == "toppra": - return ToppraPlanner(self.dof, max_constraints) + return ToppraPlanner(dofs=self.dof, max_constraints=max_constraints) else: logger.log_error( f"Unknown planner type '{planner_type}'. " @@ -253,6 +254,21 @@ def plan( ) return False, None, None, None, None, 0.0 + # pack state + init_plan_state = PlanState( + move_type=MoveType.JOINT_MOVE, + move_part=MovePart.ALL, + qpos=current_state["position"], + qvel=current_state["velocity"], + qacc=current_state["acceleration"], + ) + target_plan_states = [] + for state in target_states: + plan_state = PlanState( + move_type=MoveType.JOINT_MOVE, qpos=state["position"] + ) + target_plan_states.append(plan_state) + # Plan trajectory using selected planner ( success, @@ -262,8 +278,8 @@ def plan( times, duration, ) = self.planner.plan( - current_state=current_state, - target_states=target_states, + current_state=init_plan_state, + target_states=target_plan_states, sample_method=sample_method, sample_interval=sample_interval, ) diff --git a/embodichain/lab/sim/planners/toppra_planner.py b/embodichain/lab/sim/planners/toppra_planner.py index 5f2d0c09..b1d38230 100644 --- a/embodichain/lab/sim/planners/toppra_planner.py +++ b/embodichain/lab/sim/planners/toppra_planner.py @@ -18,6 +18,7 @@ from embodichain.utils import logger from embodichain.lab.sim.planners.utils import TrajectorySampleMethod from embodichain.lab.sim.planners.base_planner import BasePlanner +from embodichain.lab.sim.planners.utils import PlanState from typing import TYPE_CHECKING, Union, Tuple @@ -33,24 +34,25 @@ class ToppraPlanner(BasePlanner): - def __init__(self, dofs, max_constraints): + def __init__(self, **kwargs): r"""Initialize the TOPPRA trajectory planner. Args: dofs: Number of degrees of freedom max_constraints: Dictionary containing 'velocity' and 'acceleration' constraints """ - super().__init__(dofs, max_constraints) + super().__init__(**kwargs) # Create TOPPRA-specific constraint arrays (symmetric format) # This format is required by TOPPRA library + max_constraints = kwargs.get("max_constraints", None) self.vlims = np.array([[-v, v] for v in max_constraints["velocity"]]) self.alims = np.array([[-a, a] for a in max_constraints["acceleration"]]) def plan( self, - current_state: dict, - target_states: list[dict], + current_state: PlanState, + target_states: list[PlanState], **kwargs, ): r"""Execute trajectory planning. @@ -75,28 +77,25 @@ def plan( logger.log_error("At least 2 sample points required", ValueError) # Check waypoints - if len(current_state["position"]) != self.dofs: + if len(current_state.qpos) != self.dofs: logger.log_info("Current wayponit does not align") return False, None, None, None, None, None for target in target_states: - if len(target["position"]) != self.dofs: + if len(target.qpos) != self.dofs: logger.log_info("Target Wayponits does not align") return False, None, None, None, None, None if ( len(target_states) == 1 and np.sum( - np.abs( - np.array(target_states[0]["position"]) - - np.array(current_state["position"]) - ) + np.abs(np.array(target_states[0].qpos) - np.array(current_state.qpos)) ) < 1e-3 ): logger.log_info("Only two same waypoints, do not plan") return ( True, - np.array([current_state["position"], target_states[0]["position"]]), + np.array([current_state.qpos, target_states[0].qpos]), np.array([[0.0] * self.dofs, [0.0] * self.dofs]), np.array([[0.0] * self.dofs, [0.0] * self.dofs]), 0, @@ -104,11 +103,10 @@ def plan( ) # Build waypoints - waypoints = [np.array(current_state["position"])] + waypoints = [np.array(current_state.qpos)] for target in target_states: - waypoints.append(np.array(target["position"])) + waypoints.append(np.array(target.qpos)) waypoints = np.array(waypoints) - # Create spline interpolation # NOTE: Suitable for dense waypoints ss = np.linspace(0, 1, len(waypoints)) diff --git a/embodichain/lab/sim/planners/utils.py b/embodichain/lab/sim/planners/utils.py index 9b31685f..2b5cd77a 100644 --- a/embodichain/lab/sim/planners/utils.py +++ b/embodichain/lab/sim/planners/utils.py @@ -17,6 +17,9 @@ from enum import Enum from typing import Union from embodichain.utils import logger +import torch +from enum import Enum +from dataclasses import dataclass class TrajectorySampleMethod(Enum): @@ -53,3 +56,32 @@ def from_str( def __str__(self): """Override string representation for better readability.""" return self.value.capitalize() + + +class MovePart(Enum): + LEFT = 0 + RIGHT = 1 + BOTH = 2 + TORSO = 3 + ALL = 4 + + +class MoveType(Enum): + TOOL = 0 + TCP_MOVE = 1 + JOINT_MOVE = 2 + SYNC = 3 + PAUSE = 4 + + +@dataclass +class PlanState: + move_type: MoveType = MoveType.PAUSE + move_part: MovePart = MovePart.LEFT + xpos: torch.Tensor = None + qpos: torch.Tensor = None + qacc: torch.Tensor = None + qvel: torch.Tensor = None + is_open: bool = True + is_world_coordinate: bool = True + pause_seconds: float = 0.0 From f1a49efcf778a53381a3c71c5f8eac990e628246 Mon Sep 17 00:00:00 2001 From: chenjian Date: Wed, 11 Mar 2026 18:58:32 +0800 Subject: [PATCH 2/9] update --- .../lab/sim/planners/motion_generator.py | 64 +++++++------------ 1 file changed, 22 insertions(+), 42 deletions(-) diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index eb36aec5..d151b85c 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -201,8 +201,8 @@ def _create_state_dict( def plan( self, - current_state: Dict, - target_states: List[Dict], + current_state: PlanState, + target_states: List[PlanState], sample_method: TrajectorySampleMethod = TrajectorySampleMethod.TIME, sample_interval: Union[float, int] = 0.01, **kwargs, @@ -220,11 +220,8 @@ def plan( velocity and acceleration constraints, but does not check for collisions. Args: - current_state: Dictionary containing current state: - - "position": Current joint positions (required) - - "velocity": Current joint velocities (optional, defaults to zeros) - - "acceleration": Current joint accelerations (optional, defaults to zeros) - target_states: List of target state dictionaries, each with same format as current_state + current_state: PlanState + target_states: List of PlanState sample_method: Sampling method (TIME or QUANTITY) sample_interval: Sampling interval (time in seconds for TIME method, or number of points for QUANTITY) **kwargs: Additional arguments @@ -238,37 +235,6 @@ def plan( - times: np.ndarray (N,), time stamps for each point - duration: float, total trajectory duration """ - # Validate inputs - if len(current_state["position"]) != self.dof: - logger.log_warning( - f"Current state position dimension {len(current_state['position'])} " - f"does not match robot DOF {self.dof}" - ) - return False, None, None, None, None, 0.0 - - for i, target in enumerate(target_states): - if len(target["position"]) != self.dof: - logger.log_warning( - f"Target state {i} position dimension {len(target['position'])} " - f"does not match robot DOF {self.dof}" - ) - return False, None, None, None, None, 0.0 - - # pack state - init_plan_state = PlanState( - move_type=MoveType.JOINT_MOVE, - move_part=MovePart.ALL, - qpos=current_state["position"], - qvel=current_state["velocity"], - qacc=current_state["acceleration"], - ) - target_plan_states = [] - for state in target_states: - plan_state = PlanState( - move_type=MoveType.JOINT_MOVE, qpos=state["position"] - ) - target_plan_states.append(plan_state) - # Plan trajectory using selected planner ( success, @@ -278,8 +244,8 @@ def plan( times, duration, ) = self.planner.plan( - current_state=init_plan_state, - target_states=target_plan_states, + current_state=current_state, + target_states=target_states, sample_method=sample_method, sample_interval=sample_interval, ) @@ -503,10 +469,24 @@ def calculate_point_allocations( self._create_state_dict(pos) for pos in interpolate_qpos_list[1:] ] + init_plan_state = PlanState( + move_type=MoveType.JOINT_MOVE, + move_part=MovePart.ALL, + qpos=current_state["position"], + qvel=current_state["velocity"], + qacc=current_state["acceleration"], + ) + target_plan_states = [] + for state in target_states: + plan_state = PlanState( + move_type=MoveType.JOINT_MOVE, qpos=state["position"] + ) + target_plan_states.append(plan_state) + # Plan trajectory using internal plan method success, positions, velocities, accelerations, times, duration = self.plan( - current_state=current_state, - target_states=target_states, + current_state=init_plan_state, + target_states=target_plan_states, sample_method=sample_method, sample_interval=sample_num, **kwargs, From 578265c41cbb9b0a8d49fe626df064f70e07116b Mon Sep 17 00:00:00 2001 From: chenjian Date: Thu, 12 Mar 2026 12:06:15 +0800 Subject: [PATCH 3/9] add register --- .../lab/sim/planners/motion_generator.py | 67 ++++++------------- 1 file changed, 22 insertions(+), 45 deletions(-) diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index d151b85c..bd527838 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -27,13 +27,6 @@ from embodichain.lab.sim.planners.utils import PlanState, MoveType, MovePart -class PlannerType(Enum): - r"""Enumeration for different planner types.""" - - TOPPRA = "toppra" - """TOPPRA planner for time-optimal trajectory planning.""" - - class MotionGenerator: r"""Unified motion generator for robot trajectory planning. @@ -52,12 +45,16 @@ class MotionGenerator: **kwargs: Additional arguments passed to planner initialization """ + _support_planner_dict = { + "toppra": ToppraPlanner, + } + def __init__( self, robot: Robot, uid: str, sim=None, - planner_type: Union[str, PlannerType] = "toppra", + planner_type: str = "toppra", default_velocity: float = 0.2, default_acceleration: float = 0.5, collision_margin: float = 0.01, @@ -72,38 +69,16 @@ def __init__( self.dof = len(robot.get_joint_ids(uid)) # Create planner based on planner_type - self.planner_type = self._parse_planner_type(planner_type) self.planner = self._create_planner( - self.planner_type, default_velocity, default_acceleration, **kwargs + planner_type, default_velocity, default_acceleration, **kwargs ) - def _parse_planner_type(self, planner_type: Union[str, PlannerType]) -> str: - r"""Parse planner type from string or enum. - - Args: - planner_type: Planner type as string or PlannerType enum - - Returns: - Planner type as string + @classmethod + def register_planner_type(cls, name: str, planner_class): """ - if isinstance(planner_type, PlannerType): - return planner_type.value - elif isinstance(planner_type, str): - planner_type_lower = planner_type.lower() - # Validate planner type - valid_types = [e.value for e in PlannerType] - if planner_type_lower not in valid_types: - logger.log_warning( - f"Unknown planner type '{planner_type}', using 'toppra'. " - f"Valid types: {valid_types}" - ) - return "toppra" - return planner_type_lower - else: - logger.log_error( - f"planner_type must be str or PlannerType, got {type(planner_type)}", - TypeError, - ) + Register a new planner type. + """ + cls._support_planner_dict[name] = planner_class def _create_planner( self, @@ -124,18 +99,20 @@ def _create_planner( Planner instance """ # Get constraints from robot or use defaults - max_constraints = self._get_constraints( - default_velocity, default_acceleration, **kwargs - ) - - if planner_type == "toppra": - return ToppraPlanner(dofs=self.dof, max_constraints=max_constraints) - else: + planner_class = self._support_planner_dict.get(planner_type, None) + if planner_class is None: logger.log_error( - f"Unknown planner type '{planner_type}'. " - f"Supported types: {[e.value for e in PlannerType]}", + f"Unsupported planner type '{planner_type}'. " + f"Supported types: {[e for e in self._support_planner_dict.keys()]}", ValueError, ) + cfg = { + "dofs": self.dof, + "max_constraints": self._get_constraints( + default_velocity, default_acceleration, **kwargs + ), + } + return planner_class(**cfg) def _get_constraints( self, default_velocity: float, default_acceleration: float, **kwargs From ec68968e45ef3420d391d39c5c402f3184d71e00 Mon Sep 17 00:00:00 2001 From: chenjian Date: Thu, 12 Mar 2026 14:38:32 +0800 Subject: [PATCH 4/9] update --- .../lab/sim/planners/motion_generator.py | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index bd527838..faf26ecc 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -49,6 +49,13 @@ class MotionGenerator: "toppra": ToppraPlanner, } + @classmethod + def register_planner_type(cls, name: str, planner_class): + """ + Register a new planner type. + """ + cls._support_planner_dict[name] = planner_class + def __init__( self, robot: Robot, @@ -73,13 +80,6 @@ def __init__( planner_type, default_velocity, default_acceleration, **kwargs ) - @classmethod - def register_planner_type(cls, name: str, planner_class): - """ - Register a new planner type. - """ - cls._support_planner_dict[name] = planner_class - def _create_planner( self, planner_type: str, @@ -106,12 +106,12 @@ def _create_planner( f"Supported types: {[e for e in self._support_planner_dict.keys()]}", ValueError, ) - cfg = { - "dofs": self.dof, - "max_constraints": self._get_constraints( - default_velocity, default_acceleration, **kwargs - ), - } + cfg = kwargs.copy() + cfg["dofs"] = self.dof + cfg["max_constraints"] = self._get_constraints( + default_velocity, default_acceleration, **kwargs + ) + cfg["robot"] = self.robot return planner_class(**cfg) def _get_constraints( From 18bdbe9669e165d4d40dcaf90a07ba9b9c69518e Mon Sep 17 00:00:00 2001 From: chenjian Date: Thu, 12 Mar 2026 15:13:16 +0800 Subject: [PATCH 5/9] update --- embodichain/lab/sim/planners/base_planner.py | 18 ++++++++------- .../lab/sim/planners/motion_generator.py | 23 ++++++++----------- .../lab/sim/planners/toppra_planner.py | 11 +++++---- 3 files changed, 27 insertions(+), 25 deletions(-) diff --git a/embodichain/lab/sim/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index 0a5d8ebb..ed70a3f9 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -17,6 +17,7 @@ import numpy as np from abc import ABC, abstractmethod from typing import Dict, List, Tuple, Union +import torch import matplotlib.pyplot as plt from embodichain.lab.sim.planners.utils import TrajectorySampleMethod @@ -38,6 +39,7 @@ class BasePlanner(ABC): def __init__(self, **kwargs): self.dofs = kwargs.get("dofs", None) self.max_constraints = kwargs.get("max_constraints", None) + self.device = kwargs.get("device", torch.device("cpu")) @abstractmethod def plan( @@ -47,10 +49,10 @@ def plan( **kwargs, ) -> Tuple[ bool, - np.ndarray | None, - np.ndarray | None, - np.ndarray | None, - np.ndarray | None, + torch.Tensor | None, + torch.Tensor | None, + torch.Tensor | None, + torch.Tensor | None, float, ]: r"""Execute trajectory planning. @@ -65,10 +67,10 @@ def plan( Returns: Tuple of (success, positions, velocities, accelerations, times, duration): - success: bool, whether planning succeeded - - positions: np.ndarray (N, DOF), joint positions along trajectory - - velocities: np.ndarray (N, DOF), joint velocities along trajectory - - accelerations: np.ndarray (N, DOF), joint accelerations along trajectory - - times: np.ndarray (N,), time stamps for each point + - positions: torch.Tensor (N, DOF), joint positions along trajectory + - velocities: torch.Tensor (N, DOF), joint velocities along trajectory + - accelerations: torch.Tensor (N, DOF), joint accelerations along trajectory + - times: torch.Tensor (N,), time stamps for each point - duration: float, total trajectory duration """ logger.log_error("Subclasses must implement plan() method", NotImplementedError) diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index faf26ecc..09668938 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -70,7 +70,7 @@ def __init__( self.robot = robot self.sim = sim self.collision_margin = collision_margin - self.uid = uid + self.uid = uid # control part # Get robot DOF using get_joint_ids for specified control part (None for whole body) self.dof = len(robot.get_joint_ids(uid)) @@ -185,10 +185,10 @@ def plan( **kwargs, ) -> Tuple[ bool, - np.ndarray | None, - np.ndarray | None, - np.ndarray | None, - np.ndarray | None, + torch.Tensor | None, + torch.Tensor | None, + torch.Tensor | None, + torch.Tensor | None, float, ]: r"""Plan trajectory without collision checking. @@ -206,10 +206,10 @@ def plan( Returns: Tuple of (success, positions, velocities, accelerations, times, duration): - success: bool, whether planning succeeded - - positions: np.ndarray (N, DOF), joint positions along trajectory - - velocities: np.ndarray (N, DOF), joint velocities along trajectory - - accelerations: np.ndarray (N, DOF), joint accelerations along trajectory - - times: np.ndarray (N,), time stamps for each point + - positions: torch.Tensor (N, DOF), joint positions along trajectory + - velocities: torch.Tensor (N, DOF), joint velocities along trajectory + - accelerations: torch.Tensor (N, DOF), joint accelerations along trajectory + - times: torch.Tensor (N,), time stamps for each point - duration: float, total trajectory duration """ # Plan trajectory using selected planner @@ -473,10 +473,7 @@ def calculate_point_allocations( logger.log_error("Failed to plan trajectory") # Convert positions to list - out_qpos_list = ( - positions.tolist() if isinstance(positions, np.ndarray) else positions - ) - + out_qpos_list = positions.to("cpu").numpy().tolist() out_qpos_list = ( torch.tensor(out_qpos_list) if not isinstance(out_qpos_list, torch.Tensor) diff --git a/embodichain/lab/sim/planners/toppra_planner.py b/embodichain/lab/sim/planners/toppra_planner.py index b1d38230..47f3ecf1 100644 --- a/embodichain/lab/sim/planners/toppra_planner.py +++ b/embodichain/lab/sim/planners/toppra_planner.py @@ -19,6 +19,7 @@ from embodichain.lab.sim.planners.utils import TrajectorySampleMethod from embodichain.lab.sim.planners.base_planner import BasePlanner from embodichain.lab.sim.planners.utils import PlanState +import torch from typing import TYPE_CHECKING, Union, Tuple @@ -162,9 +163,11 @@ def plan( return ( True, - np.array(positions), - np.array(velocities), - np.array(accelerations), - ts, + torch.tensor(np.array(positions), dtype=torch.float32, device=self.device), + torch.tensor(np.array(velocities), dtype=torch.float32, device=self.device), + torch.tensor( + np.array(accelerations), dtype=torch.float32, device=self.device + ), + torch.tensor(ts, dtype=torch.float32, device=self.device), duration, ) From 9913aae45fb07fd56652270705cfbefa37d0b80f Mon Sep 17 00:00:00 2001 From: chenjian Date: Fri, 13 Mar 2026 16:05:41 +0800 Subject: [PATCH 6/9] update --- .../lab/sim/planners/motion_generator.py | 44 ++++++++++++++++++- embodichain/lab/sim/planners/utils.py | 30 ------------- 2 files changed, 42 insertions(+), 32 deletions(-) diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index 09668938..0d122557 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -25,6 +25,48 @@ from embodichain.lab.sim.objects.robot import Robot from embodichain.utils import logger from embodichain.lab.sim.planners.utils import PlanState, MoveType, MovePart +from dataclasses import dataclass + + +class MovePart(Enum): + """Enumeration for different robot parts to move.""" + + LEFT = 0 # left arm|eef + RIGHT = 1 # right arm|eef + BOTH = 2 # left arm|eef and right arm|eef + TORSO = 3 # torso for humanoid robot + ALL = 4 # all joints of the robot. Only for joint control. + + +class MoveType(Enum): + """Enumeration for different types of movements.""" + + TOOL = 0 # Tool open or close + TCP_MOVE = 1 # Move the end-effector to a target pose (xpos) using IK and trajectory planning + JOINT_MOVE = ( + 2 # Directly move joints to target angles (qpos) using trajectory planning + ) + SYNC = 3 # Synchronized left and right arm movement (for dual-arm robots) + PAUSE = 4 # Pause for a specified duration (use pause_seconds in PlanState) + + +@dataclass +class PlanState: + """Data class representing the state for a motion plan.""" + + move_type: MoveType = MoveType.PAUSE # Type of movement + move_part: MovePart = MovePart.LEFT # Part of the robot to move + xpos: torch.Tensor = None # target tcp pose (4x4 matrix) for TCP_MOVE + qpos: torch.Tensor = None # target joint angles for JOINT_MOVE (shape: (DOF,)) + qvel: torch.Tensor = None # target joint velocities for JOINT_MOVE (shape: (DOF,)) + qacc: torch.Tensor = ( + None # target joint accelerations for JOINT_MOVE (shape: (DOF,)) + ) + is_open: bool = True # for TOOL move type, whether to open or close the tool + is_world_coordinate: bool = ( + True # whether the target pose is in world coordinates (True) or relative to current pose (False) + ) + pause_seconds: float = 0.0 # duration to pause for PAUSE move type class MotionGenerator: @@ -60,7 +102,6 @@ def __init__( self, robot: Robot, uid: str, - sim=None, planner_type: str = "toppra", default_velocity: float = 0.2, default_acceleration: float = 0.5, @@ -68,7 +109,6 @@ def __init__( **kwargs, ): self.robot = robot - self.sim = sim self.collision_margin = collision_margin self.uid = uid # control part diff --git a/embodichain/lab/sim/planners/utils.py b/embodichain/lab/sim/planners/utils.py index 2b5cd77a..7892a0d5 100644 --- a/embodichain/lab/sim/planners/utils.py +++ b/embodichain/lab/sim/planners/utils.py @@ -19,7 +19,6 @@ from embodichain.utils import logger import torch from enum import Enum -from dataclasses import dataclass class TrajectorySampleMethod(Enum): @@ -56,32 +55,3 @@ def from_str( def __str__(self): """Override string representation for better readability.""" return self.value.capitalize() - - -class MovePart(Enum): - LEFT = 0 - RIGHT = 1 - BOTH = 2 - TORSO = 3 - ALL = 4 - - -class MoveType(Enum): - TOOL = 0 - TCP_MOVE = 1 - JOINT_MOVE = 2 - SYNC = 3 - PAUSE = 4 - - -@dataclass -class PlanState: - move_type: MoveType = MoveType.PAUSE - move_part: MovePart = MovePart.LEFT - xpos: torch.Tensor = None - qpos: torch.Tensor = None - qacc: torch.Tensor = None - qvel: torch.Tensor = None - is_open: bool = True - is_world_coordinate: bool = True - pause_seconds: float = 0.0 From 7491340d60e7596165a8ae8ce5a57c999b07530d Mon Sep 17 00:00:00 2001 From: chenjian Date: Fri, 13 Mar 2026 17:15:08 +0800 Subject: [PATCH 7/9] fix --- embodichain/lab/sim/planners/motion_generator.py | 1 - 1 file changed, 1 deletion(-) diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index 0d122557..a9c50f34 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -24,7 +24,6 @@ from embodichain.lab.sim.planners.utils import TrajectorySampleMethod from embodichain.lab.sim.objects.robot import Robot from embodichain.utils import logger -from embodichain.lab.sim.planners.utils import PlanState, MoveType, MovePart from dataclasses import dataclass From 4d7552474001264901c21bbd2754c2103ce2aef8 Mon Sep 17 00:00:00 2001 From: chenjian Date: Fri, 13 Mar 2026 18:07:45 +0800 Subject: [PATCH 8/9] fix --- embodichain/lab/sim/planners/base_planner.py | 2 +- embodichain/lab/sim/planners/toppra_planner.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/embodichain/lab/sim/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index ed70a3f9..155828ec 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -22,7 +22,7 @@ from embodichain.lab.sim.planners.utils import TrajectorySampleMethod from embodichain.utils import logger -from embodichain.lab.sim.planners.utils import PlanState +from embodichain.lab.sim.planners.motion_generator import PlanState class BasePlanner(ABC): diff --git a/embodichain/lab/sim/planners/toppra_planner.py b/embodichain/lab/sim/planners/toppra_planner.py index 47f3ecf1..1e59c581 100644 --- a/embodichain/lab/sim/planners/toppra_planner.py +++ b/embodichain/lab/sim/planners/toppra_planner.py @@ -18,7 +18,7 @@ from embodichain.utils import logger from embodichain.lab.sim.planners.utils import TrajectorySampleMethod from embodichain.lab.sim.planners.base_planner import BasePlanner -from embodichain.lab.sim.planners.utils import PlanState +from embodichain.lab.sim.planners.motion_generator import PlanState import torch from typing import TYPE_CHECKING, Union, Tuple From cbf9202ee52a0617ade305065656d740e1dcaccc Mon Sep 17 00:00:00 2001 From: chenjian Date: Fri, 13 Mar 2026 18:57:25 +0800 Subject: [PATCH 9/9] update --- embodichain/lab/sim/planners/base_planner.py | 2 +- .../lab/sim/planners/motion_generator.py | 43 +------------------ .../lab/sim/planners/toppra_planner.py | 2 +- embodichain/lab/sim/planners/utils.py | 42 ++++++++++++++++++ 4 files changed, 45 insertions(+), 44 deletions(-) diff --git a/embodichain/lab/sim/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index 155828ec..5f8ab004 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -22,7 +22,7 @@ from embodichain.lab.sim.planners.utils import TrajectorySampleMethod from embodichain.utils import logger -from embodichain.lab.sim.planners.motion_generator import PlanState +from .utils import MovePart, MoveType, PlanState class BasePlanner(ABC): diff --git a/embodichain/lab/sim/planners/motion_generator.py b/embodichain/lab/sim/planners/motion_generator.py index a9c50f34..2a75fe51 100644 --- a/embodichain/lab/sim/planners/motion_generator.py +++ b/embodichain/lab/sim/planners/motion_generator.py @@ -24,48 +24,7 @@ from embodichain.lab.sim.planners.utils import TrajectorySampleMethod from embodichain.lab.sim.objects.robot import Robot from embodichain.utils import logger -from dataclasses import dataclass - - -class MovePart(Enum): - """Enumeration for different robot parts to move.""" - - LEFT = 0 # left arm|eef - RIGHT = 1 # right arm|eef - BOTH = 2 # left arm|eef and right arm|eef - TORSO = 3 # torso for humanoid robot - ALL = 4 # all joints of the robot. Only for joint control. - - -class MoveType(Enum): - """Enumeration for different types of movements.""" - - TOOL = 0 # Tool open or close - TCP_MOVE = 1 # Move the end-effector to a target pose (xpos) using IK and trajectory planning - JOINT_MOVE = ( - 2 # Directly move joints to target angles (qpos) using trajectory planning - ) - SYNC = 3 # Synchronized left and right arm movement (for dual-arm robots) - PAUSE = 4 # Pause for a specified duration (use pause_seconds in PlanState) - - -@dataclass -class PlanState: - """Data class representing the state for a motion plan.""" - - move_type: MoveType = MoveType.PAUSE # Type of movement - move_part: MovePart = MovePart.LEFT # Part of the robot to move - xpos: torch.Tensor = None # target tcp pose (4x4 matrix) for TCP_MOVE - qpos: torch.Tensor = None # target joint angles for JOINT_MOVE (shape: (DOF,)) - qvel: torch.Tensor = None # target joint velocities for JOINT_MOVE (shape: (DOF,)) - qacc: torch.Tensor = ( - None # target joint accelerations for JOINT_MOVE (shape: (DOF,)) - ) - is_open: bool = True # for TOOL move type, whether to open or close the tool - is_world_coordinate: bool = ( - True # whether the target pose is in world coordinates (True) or relative to current pose (False) - ) - pause_seconds: float = 0.0 # duration to pause for PAUSE move type +from .utils import MovePart, MoveType, PlanState class MotionGenerator: diff --git a/embodichain/lab/sim/planners/toppra_planner.py b/embodichain/lab/sim/planners/toppra_planner.py index 1e59c581..7d90a205 100644 --- a/embodichain/lab/sim/planners/toppra_planner.py +++ b/embodichain/lab/sim/planners/toppra_planner.py @@ -18,7 +18,7 @@ from embodichain.utils import logger from embodichain.lab.sim.planners.utils import TrajectorySampleMethod from embodichain.lab.sim.planners.base_planner import BasePlanner -from embodichain.lab.sim.planners.motion_generator import PlanState +from .utils import MovePart, MoveType, PlanState import torch from typing import TYPE_CHECKING, Union, Tuple diff --git a/embodichain/lab/sim/planners/utils.py b/embodichain/lab/sim/planners/utils.py index 7892a0d5..049941a1 100644 --- a/embodichain/lab/sim/planners/utils.py +++ b/embodichain/lab/sim/planners/utils.py @@ -19,6 +19,7 @@ from embodichain.utils import logger import torch from enum import Enum +from dataclasses import dataclass class TrajectorySampleMethod(Enum): @@ -55,3 +56,44 @@ def from_str( def __str__(self): """Override string representation for better readability.""" return self.value.capitalize() + + +class MovePart(Enum): + """Enumeration for different robot parts to move.""" + + LEFT = 0 # left arm|eef + RIGHT = 1 # right arm|eef + BOTH = 2 # left arm|eef and right arm|eef + TORSO = 3 # torso for humanoid robot + ALL = 4 # all joints of the robot. Only for joint control. + + +class MoveType(Enum): + """Enumeration for different types of movements.""" + + TOOL = 0 # Tool open or close + TCP_MOVE = 1 # Move the end-effector to a target pose (xpos) using IK and trajectory planning + JOINT_MOVE = ( + 2 # Directly move joints to target angles (qpos) using trajectory planning + ) + SYNC = 3 # Synchronized left and right arm movement (for dual-arm robots) + PAUSE = 4 # Pause for a specified duration (use pause_seconds in PlanState) + + +@dataclass +class PlanState: + """Data class representing the state for a motion plan.""" + + move_type: MoveType = MoveType.PAUSE # Type of movement + move_part: MovePart = MovePart.LEFT # Part of the robot to move + xpos: torch.Tensor = None # target tcp pose (4x4 matrix) for TCP_MOVE + qpos: torch.Tensor = None # target joint angles for JOINT_MOVE (shape: (DOF,)) + qvel: torch.Tensor = None # target joint velocities for JOINT_MOVE (shape: (DOF,)) + qacc: torch.Tensor = ( + None # target joint accelerations for JOINT_MOVE (shape: (DOF,)) + ) + is_open: bool = True # for TOOL move type, whether to open or close the tool + is_world_coordinate: bool = ( + True # whether the target pose is in world coordinates (True) or relative to current pose (False) + ) + pause_seconds: float = 0.0 # duration to pause for PAUSE move type