44import mujoco
55import numpy as np
66from rcsss ._core .common import Pose
7- from rcsss ._core .sim import FR3State
87from rcsss .envs .base import GripperWrapper
98
109logger = logging .getLogger (__name__ )
@@ -34,9 +33,7 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in
3433 return waypoints
3534
3635 def step (self , action : np .ndarray ) -> dict :
37- re = self .env .step (action )
38- s : FR3State = self .env .unwrapped .robot .get_state ()
39- return re [0 ]
36+ return self .env .step (action )[0 ]
4037
4138 def plan_linear_motion (self , geom_name : str , delta_up : float , num_waypoints : int = 20 ) -> list [Pose ]:
4239 end_eff_pose = self .env .unwrapped .robot .get_cartesian_position ()
@@ -47,8 +44,7 @@ def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int
4744 # this does not work if the object is flipped
4845 goal_pose *= Pose (translation = [0 , 0 , delta_up ], quaternion = [1 , 0 , 0 , 0 ])
4946
50- waypoints = self .generate_waypoints (end_eff_pose , goal_pose , num_waypoints = num_waypoints )
51- return waypoints
47+ return self .generate_waypoints (end_eff_pose , goal_pose , num_waypoints = num_waypoints )
5248
5349 def execute_motion (self , waypoints : list [Pose ], gripper : float = GripperWrapper .BINARY_GRIPPER_OPEN ) -> dict :
5450 for i in range (1 , len (waypoints )):
0 commit comments