diff --git a/benchmarks/panda_arm/.gitignore b/benchmarks/panda_arm/.gitignore new file mode 100644 index 0000000..73c8fe6 --- /dev/null +++ b/benchmarks/panda_arm/.gitignore @@ -0,0 +1 @@ +jit_* diff --git a/benchmarks/panda_arm/mppi_planner/__init__.py b/benchmarks/panda_arm/mppi_planner/__init__.py new file mode 100644 index 0000000..4c89e6c --- /dev/null +++ b/benchmarks/panda_arm/mppi_planner/__init__.py @@ -0,0 +1 @@ +from .mppi_planner_wrapper import MPPIPlanner diff --git a/benchmarks/panda_arm/mppi_planner/mppi_planner_wrapper.py b/benchmarks/panda_arm/mppi_planner/mppi_planner_wrapper.py new file mode 100644 index 0000000..023105d --- /dev/null +++ b/benchmarks/panda_arm/mppi_planner/mppi_planner_wrapper.py @@ -0,0 +1,132 @@ +from omegaconf import OmegaConf +from plannerbenchmark.generic.planner import Planner +from mppiisaac.planner.isaacgym_wrapper import IsaacGymWrapper +from mppiisaac.planner.mppi_isaac import MPPIisaacPlanner + +import torch + +class ObjectiveOld(object): + def __init__(self, goal, device): + self.nav_goal = torch.tensor(goal, device=device) + + self.w_nav = 2.0 + self.w_obs = 1 + + def compute_cost(self, sim: IsaacGymWrapper): + dof_state = sim.dof_state + pos = torch.cat((dof_state[:, 0].unsqueeze(1), dof_state[:, 2].unsqueeze(1)), 1) + obs_positions = sim.obstacle_positions + + nav_cost = torch.clamp( + torch.linalg.norm(pos - self.nav_goal, axis=1), min=0, max=1999 + ) + + # sim.gym.refresh_net_contact_force_tensor(sim.sim) + # sim.net_cf + + # This can cause steady state error if the goal is close to an obstacle, better use contact forces later on + obs_cost = torch.sum( + 1 / torch.linalg.norm(obs_positions[:, :, :2] - pos.unsqueeze(1), axis=2), + axis=1, + ) + + return nav_cost * self.w_nav + obs_cost * self.w_obs + +class Objective(object): + def __init__(self, goal, device): + self.nav_goal = torch.tensor(goal, device=device) + + self.w_nav = 1.0 + self.w_obs = 0.5 + + def compute_cost(self, sim: IsaacGymWrapper): + dof_state = sim.dof_state + pos = torch.cat((dof_state[:, 0].unsqueeze(1), dof_state[:, 2].unsqueeze(1)), 1) + obs_positions = sim.obstacle_positions + + nav_cost = torch.clamp( + torch.linalg.norm(pos - self.nav_goal, axis=1) - 0.05, min=0, max=1999 + ) + + # sim.gym.refresh_net_contact_force_tensor(sim.sim) + # sim.net_cf + + # This can cause steady state error if the goal is close to an obstacle, better use contact forces later on + obs_cost = torch.sum( + 1 / torch.linalg.norm(obs_positions[:, :, :2] - pos.unsqueeze(1), axis=2), + axis=1, + ) + + return nav_cost * self.w_nav + obs_cost * self.w_obs + +class EndEffectorGoalObjective(object): + def __init__(self, goal, device): + self.nav_goal = torch.tensor(goal, device=device) + self.ort_goal = torch.tensor([1, 0, 0, 0], device=device) + + def compute_cost(self, sim): + pos = sim.rigid_body_state[:, sim.robot_rigid_body_ee_idx, :3] + ort = sim.rigid_body_state[:, sim.robot_rigid_body_ee_idx, 3:7] + # dof_states = gym.acquire_dof_state_tensor(sim) + + reach_cost = torch.linalg.norm(pos - self.nav_goal, axis=1) + align_cost = torch.linalg.norm(ort - self.ort_goal, axis=1) + return 10 * reach_cost + align_cost + # return torch.clamp( + # torch.linalg.norm(pos - self.nav_goal, axis=1) - 0.05, min=0, max=1999 + # ) + + + + + +class MPPIPlanner(Planner): + + def __init__(self, exp, **kwargs): + super().__init__(exp, **kwargs) + self.cfg = kwargs['config'] + initial_actor_position = exp.initState()[0].tolist() + initial_actor_position[2] += 0.05 + initial_actor_position = [0.0, 0.0, 0.05] + self.cfg['initial_actor_positions'] = [initial_actor_position] + self._config = OmegaConf.create(kwargs) + + self.reset() + + def setJointLimits(self, limits): + self._limits = limits + + def setGoal(self, motionPlanningGoal): + cfg = OmegaConf.create(self.cfg) + goal_position = motionPlanningGoal.sub_goals()[0].position() + objective = EndEffectorGoalObjective(goal_position, cfg.mppi.device) + if not hasattr(self, '_planner'): + self._planner = MPPIisaacPlanner(cfg, objective) + + + def setSelfCollisionAvoidance(self, r_body): + pass + + def setObstacles(self, obstacles, r_body): + pass + + def concretize(self): + pass + + def save(self, folderPath): + file_name = folderPath + "/planner.yaml" + OmegaConf.save(config=self._config, f=file_name) + + def computeAction(self, **kwargs): + ob = kwargs + obst = ob["FullSensor"]["obstacles"] + for o in obst.values(): + o['type'] = 'sphere' + + action = self._planner.compute_action( + q=ob["joint_state"]["position"], + qdot=ob["joint_state"]["velocity"], + obst=obst, + ) + return action.numpy() + diff --git a/benchmarks/panda_arm/results/.gitignore b/benchmarks/panda_arm/results/.gitignore new file mode 100644 index 0000000..150f68c --- /dev/null +++ b/benchmarks/panda_arm/results/.gitignore @@ -0,0 +1 @@ +*/* diff --git a/benchmarks/panda_arm/setup/acados_mpc.yaml b/benchmarks/panda_arm/setup/acados_mpc.yaml new file mode 100644 index 0000000..ac123bf --- /dev/null +++ b/benchmarks/panda_arm/setup/acados_mpc.yaml @@ -0,0 +1,7 @@ +name: AcadosMpc +N: 20 +w_pos: 4.0 +w_coll: 0.00 +w_input: 0.01 +w_joints: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] +w_vref: 5.0 diff --git a/benchmarks/panda_arm/setup/exp.yaml b/benchmarks/panda_arm/setup/exp.yaml new file mode 100644 index 0000000..35b2c4c --- /dev/null +++ b/benchmarks/panda_arm/setup/exp.yaml @@ -0,0 +1,108 @@ +T: 2000 +dt: 0.01 +urdf_file_name: "panda.urdf" +base_type: "holonomic" +root_link : "panda_link0" +control_mode: 'acc' +ee_links: + - "panda_link7" +robot_type: panda +n: 7 +goal: + subgoal1: + is_primary_goal: True + weight: 1 + indices: + - 0 + - 1 + - 2 + parent_link: "panda_link0" + child_link: "panda_link7" + desired_position: + - 0.5 + - -0.4 + - 0.3 + low: + - -0.4 + - -0.4 + - 0.1 + high: + - 0.7 + - 0.0 + - 1.1 + type: staticSubGoal + epsilon: 0.05 +initState: + q0: + - 1.0 + - 0.0 + - 0.0 + - -1.50 + - 0.0 + - 1.8675 + - 0.0 + q0dot: + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 +limits: + low: + - -2.8973 + - -1.7628 + - -2.8973 + - -3.0718 + - -2.8973 + - -0.0175 + - -2.8973 + high: + - 2.8973 + - 1.7628 + - 2.8973 + - 0.0698 + - 2.8973 + - 3.7525 + - 2.8973 +r_body: 0.15 +selfCollision: + pairs: + - ["panda_link0", "panda_link6"] + - ["panda_link0", "panda_link7"] + - ["panda_link1", "panda_link6"] + - ["panda_link1", "panda_link7"] + - ["panda_link2", "panda_link6"] + - ["panda_link2", "panda_link7"] +collision_links: + - "panda_link2" + - "panda_link3" + - "panda_link5" + - "panda_link7" +obstacles: + obst0: + geometry: + position: + - 0.5 + - 0.0 + - 0.6 + radius: 0.15 + type: sphere + high: + position: + - 1.0 + - 1.0 + - 1.0 + radius: + 0.3 + low: + position: + - 0.0 + - -1.0 + - 0.0 + radius: + 0.1 +randomObstacles: + number: 5 + diff --git a/benchmarks/panda_arm/setup/fabric.yaml b/benchmarks/panda_arm/setup/fabric.yaml new file mode 100644 index 0000000..118fe81 --- /dev/null +++ b/benchmarks/panda_arm/setup/fabric.yaml @@ -0,0 +1,25 @@ +interval: 1 +name: fabric +robot_type: simPanda +n: 7 +m_base: 1.0 +obst: + exp: 1.0 + lam: 1.0 +selfCol: + exp: 3.0 + lam: 2.0 +attractor: + k_psi: 2.0 +limits: + exp: 1.0 + lam: 1.0 +speed: + ex_factor: 1.0 +damper: + r_d: 0.05 + b: + - 0.01 + - 3.5 +dynamic: False + diff --git a/benchmarks/panda_arm/setup/mpc.yaml b/benchmarks/panda_arm/setup/mpc.yaml new file mode 100644 index 0000000..d18631e --- /dev/null +++ b/benchmarks/panda_arm/setup/mpc.yaml @@ -0,0 +1,13 @@ +time_horizon: 30 +time_step: 0.1 +interval: 10 +n: 7 +number_obstacles: 5 +slack: true +name: forcesprompc +model_name: panda +weights: + ws: 1e10 + wu: 1.0 + wvel: 1.0 + wx: 10.0 diff --git a/benchmarks/panda_arm/setup/mppi.yaml b/benchmarks/panda_arm/setup/mppi.yaml new file mode 100644 index 0000000..d3bfa02 --- /dev/null +++ b/benchmarks/panda_arm/setup/mppi.yaml @@ -0,0 +1,102 @@ +name: mppi +config: + render: true + n_steps: 1000 + mppi: + num_samples: 100 + horizon: 12 + mppi_mode: halton-spline + sampling_method: halton + noise_sigma: + - - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + noise_mu: null + device: cuda:0 + lambda_: 0.1 + update_lambda: false + update_cov: false + u_min: + - -2 + u_max: + - 2 + u_init: 0.0 + U_init: null + u_scale: 1.0 + u_per_command: 1 + rollout_var_discount: 0.99 + sample_null_action: true + noise_abs_cost: false + filter_u: true + use_priors: true + isaacgym: + dt: 0.05 + substeps: 1 + use_gpu_pipeline: true + num_client_threads: 0 + viewer: false + num_obstacles: 10 + spacing: 6.0 + goal: + - 2.0 + - 2.0 + nx: 6 + urdf_file: point_robot.urdf + fix_base: true + flip_visual: false + disable_gravity: false + differential_drive: false + wheel_base: 0.0 + wheel_radius: 0.0 + wheel_count: 0 + ee_link: null + actors: + - 'panda' + - 'xaxis' + - 'yaxis' diff --git a/benchmarks/point_robot/README.md b/benchmarks/point_robot/README.md new file mode 100644 index 0000000..669d664 --- /dev/null +++ b/benchmarks/point_robot/README.md @@ -0,0 +1,9 @@ +# Benchmark for point robot + +If you want to compare mppi to other planners, you can use plannerbenchmark. +As mppi is not part of the standard planners, you must export the following +variable. + +```bash +export LOCAL_PLANNER_BENCH_CUSTOM=/path/to/mppi_isaac/benchmarks/point_robot/mppi_planner +``` diff --git a/benchmarks/point_robot/mppi_planner/__init__.py b/benchmarks/point_robot/mppi_planner/__init__.py new file mode 100644 index 0000000..4c89e6c --- /dev/null +++ b/benchmarks/point_robot/mppi_planner/__init__.py @@ -0,0 +1 @@ +from .mppi_planner_wrapper import MPPIPlanner diff --git a/benchmarks/point_robot/mppi_planner/mppi_planner_wrapper.py b/benchmarks/point_robot/mppi_planner/mppi_planner_wrapper.py new file mode 100644 index 0000000..41da2dd --- /dev/null +++ b/benchmarks/point_robot/mppi_planner/mppi_planner_wrapper.py @@ -0,0 +1,113 @@ +from omegaconf import OmegaConf +from plannerbenchmark.generic.planner import Planner +from mppiisaac.planner.isaacgym_wrapper import IsaacGymWrapper +from mppiisaac.planner.mppi_isaac import MPPIisaacPlanner + +import torch + +class ObjectiveOld(object): + def __init__(self, goal, device): + self.nav_goal = torch.tensor(goal, device=device) + + self.w_nav = 2.0 + self.w_obs = 1 + + def compute_cost(self, sim: IsaacGymWrapper): + dof_state = sim.dof_state + pos = torch.cat((dof_state[:, 0].unsqueeze(1), dof_state[:, 2].unsqueeze(1)), 1) + obs_positions = sim.obstacle_positions + + nav_cost = torch.clamp( + torch.linalg.norm(pos - self.nav_goal, axis=1), min=0, max=1999 + ) + + # sim.gym.refresh_net_contact_force_tensor(sim.sim) + # sim.net_cf + + # This can cause steady state error if the goal is close to an obstacle, better use contact forces later on + obs_cost = torch.sum( + 1 / torch.linalg.norm(obs_positions[:, :, :2] - pos.unsqueeze(1), axis=2), + axis=1, + ) + + return nav_cost * self.w_nav + obs_cost * self.w_obs + +class Objective(object): + def __init__(self, goal, device): + self.nav_goal = torch.tensor(goal, device=device) + + self.w_nav = 1.0 + self.w_obs = 0.5 + + def compute_cost(self, sim: IsaacGymWrapper): + dof_state = sim.dof_state + pos = torch.cat((dof_state[:, 0].unsqueeze(1), dof_state[:, 2].unsqueeze(1)), 1) + obs_positions = sim.obstacle_positions + + nav_cost = torch.clamp( + torch.linalg.norm(pos - self.nav_goal, axis=1) - 0.05, min=0, max=1999 + ) + + # sim.gym.refresh_net_contact_force_tensor(sim.sim) + # sim.net_cf + + # This can cause steady state error if the goal is close to an obstacle, better use contact forces later on + obs_cost = torch.sum( + 1 / torch.linalg.norm(obs_positions[:, :, :2] - pos.unsqueeze(1), axis=2), + axis=1, + ) + + return nav_cost * self.w_nav + obs_cost * self.w_obs + + + + +class MPPIPlanner(Planner): + + def __init__(self, exp, **kwargs): + super().__init__(exp, **kwargs) + self.cfg = kwargs['config'] + initial_actor_position = exp.initState()[0].tolist() + initial_actor_position[2] += 0.05 + self.cfg['initial_actor_positions'] = [initial_actor_position] + self._config = OmegaConf.create(kwargs) + + self.reset() + + def setJointLimits(self, limits): + self._limits = limits + + def setGoal(self, motionPlanningGoal): + cfg = OmegaConf.create(self.cfg) + goal_position = motionPlanningGoal.sub_goals()[0].position() + objective = Objective(goal_position, cfg.mppi.device) + if not hasattr(self, '_planner'): + self._planner = MPPIisaacPlanner(cfg, objective) + + + def setSelfCollisionAvoidance(self, r_body): + pass + + def setObstacles(self, obstacles, r_body): + pass + + def concretize(self): + pass + + def save(self, folderPath): + file_name = folderPath + "/planner.yaml" + OmegaConf.save(config=self._config, f=file_name) + + def computeAction(self, **kwargs): + ob = kwargs + obst = ob["FullSensor"]["obstacles"] + for o in obst.values(): + o['type'] = 'sphere' + + action = self._planner.compute_action( + q=ob["joint_state"]["position"], + qdot=ob["joint_state"]["velocity"], + obst=obst, + ) + return action.numpy() + diff --git a/benchmarks/point_robot/results/.gitignore b/benchmarks/point_robot/results/.gitignore new file mode 100644 index 0000000..d6b7ef3 --- /dev/null +++ b/benchmarks/point_robot/results/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore diff --git a/benchmarks/point_robot/setup/exp.yaml b/benchmarks/point_robot/setup/exp.yaml new file mode 100644 index 0000000..2712e38 --- /dev/null +++ b/benchmarks/point_robot/setup/exp.yaml @@ -0,0 +1,86 @@ +T: 800 +dynamic: False +dt: 0.05 +root_link : "world" +base_type: "holonomic" +urdf_file_name : "point_robot.urdf" +ee_links: + - 'base_link' +control_mode: 'vel' +n: 3 +goal: + subgoal0: + is_primary_goal: True + weight: 1 + indices: + - 0 + - 1 + parent_link: "world_link" + child_link: "base_link" + desired_position: + - 4.5 + - 0.2 + low: + - -5.0 + - -5.0 + high: + - 5.0 + - 5.0 + type: staticSubGoal + epsilon: 0.1 +initState: + q0: + - 0.1 + - 0.0 + - 0.0 + q0dot: + - 0.0 + - 0.0 + - 0.0 +limits: + low: + - -5.0 + - -5.0 + - -5.0 + high: + - 5.0 + - 5.0 + - 5.0 +r_body: 0.2 +randomObstacles: + number: 3 +obstacles: + obst0: + type: sphere + geometry: + position: + - 2.0 + - 0.4 + - 0.0 + radius: 0.7 + low: + position: + - 1.0 + - -1.0 + - 0.0 + radius: 0.1 + high: + position: + - 3.0 + - 1.0 + - 0.0 + radius: 0.5 + obst1: + type: sphere + geometry: + position: + - 20.4 + - 0.4 + - 0.0 + radius: 1.0 +selfCollision: + pairs: +collision_links: + - 'base_link' + + diff --git a/benchmarks/point_robot/setup/fabric.yaml b/benchmarks/point_robot/setup/fabric.yaml new file mode 100644 index 0000000..34741a9 --- /dev/null +++ b/benchmarks/point_robot/setup/fabric.yaml @@ -0,0 +1,24 @@ +interval: 1 +name: fabric +n: 3 +m_base: 0.2 +obst: + exp: 1.0 + lam: 1.0 +selfCol: + exp: 1.0 + lam: 2.0 +attractor: + k_psi: 1.0 +speed: + ex_factor: 5.0 +limits: + lam: 10.0 + exp: 3.0 +damper: + r_d: 0.80 + b: + - 0.1 + - 6.5 +dynamic: False + diff --git a/benchmarks/point_robot/setup/mpc.yaml b/benchmarks/point_robot/setup/mpc.yaml new file mode 100644 index 0000000..8183b07 --- /dev/null +++ b/benchmarks/point_robot/setup/mpc.yaml @@ -0,0 +1,14 @@ +time_horizon: 10 +time_step: 0.5 +interval: 10 +n: 3 +number_obstacles: 5 +name: forcesprompc +model_name: po1nt_robot +slack: False +weights: + ws: 1e10 + wu: 1.0 + wvel: 1.0 + wx: 1.5 + wobst: 0.2 diff --git a/benchmarks/point_robot/setup/mppi.yaml b/benchmarks/point_robot/setup/mppi.yaml new file mode 100644 index 0000000..4feb028 --- /dev/null +++ b/benchmarks/point_robot/setup/mppi.yaml @@ -0,0 +1,60 @@ +name: mppi +config: + render: true + n_steps: 1000 + mppi: + num_samples: 100 + horizon: 12 + mppi_mode: halton-spline + sampling_method: halton + noise_sigma: + - - 1.0 + - 0.0 + - 0.0 + - - 0.0 + - 1.0 + - 0.0 + - - 0.0 + - 0.0 + - 1.0 + noise_mu: null + device: cuda:0 + lambda_: 0.1 + update_lambda: false + update_cov: false + u_min: + - -2 + u_max: + - 2 + u_init: 0.0 + U_init: null + u_scale: 1.0 + u_per_command: 1 + rollout_var_discount: 0.99 + sample_null_action: true + noise_abs_cost: false + filter_u: true + use_priors: true + isaacgym: + dt: 0.05 + substeps: 1 + use_gpu_pipeline: true + num_client_threads: 0 + viewer: false + num_obstacles: 10 + spacing: 6.0 + goal: + - 2.0 + - 2.0 + nx: 6 + urdf_file: point_robot.urdf + fix_base: true + flip_visual: false + disable_gravity: false + differential_drive: false + wheel_base: 0.0 + wheel_radius: 0.0 + wheel_count: 0 + ee_link: null + actors: + - 'point_robot' diff --git a/examples/jackal_robot.py b/examples/jackal_robot.py index a277270..e5d029f 100644 --- a/examples/jackal_robot.py +++ b/examples/jackal_robot.py @@ -137,6 +137,7 @@ def run_jackal_robot(cfg: ExampleConfig): ob, *_, ) = env.step(action) + print(action) return {} diff --git a/examples/panda_robot_with_obstacles.py b/examples/panda_robot_with_obstacles.py index 224a370..1aa3fe5 100644 --- a/examples/panda_robot_with_obstacles.py +++ b/examples/panda_robot_with_obstacles.py @@ -63,7 +63,7 @@ def initalize_environment(cfg): robots = [ GenericUrdfReacher(urdf=urdf_file, mode="vel"), ] - env: UrdfEnv = gym.make("urdf-env-v0", dt=0.01, robots=robots, render=cfg.render) + env: UrdfEnv = gym.make("urdf-env-v0", dt=0.01, robots=robots, render=cfg.render, observation_checking=False) # Set the initial position and velocity of the panda arm. env.reset() @@ -97,9 +97,12 @@ def initalize_environment(cfg): # sense both sensor = FullSensor( - goal_mask=["position"], obstacle_mask=["position", "velocity", "type", "size"] + goal_mask=["position"], + obstacle_mask=["position", "velocity", "size"], + variance=0.0, ) env.add_sensor(sensor, [0]) + env.set_spaces() return env diff --git a/examples/point_robot_with_obstacle.py b/examples/point_robot_with_obstacle.py index 2bb63b7..5298696 100644 --- a/examples/point_robot_with_obstacle.py +++ b/examples/point_robot_with_obstacle.py @@ -68,7 +68,7 @@ def initalize_environment(cfg) -> UrdfEnv: robots = [ GenericUrdfReacher(urdf=urdf_file, mode="vel"), ] - env: UrdfEnv = gym.make("urdf-env-v0", dt=0.05, robots=robots, render=cfg.render) + env: UrdfEnv = gym.make("urdf-env-v0", dt=0.05, robots=robots, render=cfg.render, observation_checking=False) # Set the initial position and velocity of the point mass. env.reset() @@ -81,17 +81,18 @@ def initalize_environment(cfg) -> UrdfEnv: sphereObst = SphereObstacle(name="simpleSphere", content_dict=obst1Dict) env.add_obstacle(sphereObst) - obst2Dict = { - "type": "box", - "geometry": { - "position": [1.0, 2.0, 0.0], - "width": 0.3, - "height": 0.2, - "length": 1.0, - }, - } - boxObst = BoxObstacle(name="simpleBox", content_dict=obst2Dict) - env.add_obstacle(boxObst) + # TODO: Allow for non-sphere obstacles. Wait for update of urdfenvs. + # obst2Dict = { + # "type": "box", + # "geometry": { + # "position": [1.0, 2.0, 0.0], + # "width": 0.3, + # "height": 0.2, + # "length": 1.0, + # }, + # } + # boxObst = BoxObstacle(name="simpleBox", content_dict=obst2Dict) + # env.add_obstacle(boxObst) goal_dict = { "weight": 1.0, "is_primary_goal": True, @@ -107,9 +108,12 @@ def initalize_environment(cfg) -> UrdfEnv: # sense both sensor = FullSensor( - goal_mask=["position"], obstacle_mask=["position", "velocity", "type", "size"] + goal_mask=["position"], + obstacle_mask=["position", "velocity", "size"], + variance=0.0, ) env.add_sensor(sensor, [0]) + env.set_spaces() return env @@ -150,6 +154,7 @@ def run_point_robot(cfg: ExampleConfig): """ # Note: Workaround to trigger the dataclasses __post_init__ method cfg = OmegaConf.to_object(cfg) + OmegaConf.save(config=cfg, f='test.yaml') env = initalize_environment(cfg) planner = set_planner(cfg) diff --git a/mppiisaac/planner/isaacgym_wrapper.py b/mppiisaac/planner/isaacgym_wrapper.py index d3afc7e..492fb44 100644 --- a/mppiisaac/planner/isaacgym_wrapper.py +++ b/mppiisaac/planner/isaacgym_wrapper.py @@ -487,8 +487,11 @@ def update_root_state_tensor_by_obstacles(self, obstacles): """ env_cfg_changed = False - for i, obst in enumerate(obstacles): - pos, vel, o_type, o_size = obst + for i, obst in enumerate(list(obstacles.values())): + pos = obst['position'] + vel = obst['velocity'] + o_type = 'sphere' + o_size = obst['size'] name = f"{o_type}{i}" try: obst_idx = [ diff --git a/poetry.lock b/poetry.lock index d384cc8..f55cf6c 100644 --- a/poetry.lock +++ b/poetry.lock @@ -13,14 +13,14 @@ files = [ [[package]] name = "astroid" -version = "2.15.3" +version = "2.15.4" description = "An abstract syntax tree for Python with inference support." category = "dev" optional = false python-versions = ">=3.7.2" files = [ - {file = "astroid-2.15.3-py3-none-any.whl", hash = "sha256:f11e74658da0f2a14a8d19776a8647900870a63de71db83713a8e77a6af52662"}, - {file = "astroid-2.15.3.tar.gz", hash = "sha256:44224ad27c54d770233751315fa7f74c46fa3ee0fab7beef1065f99f09897efe"}, + {file = "astroid-2.15.4-py3-none-any.whl", hash = "sha256:a1b8543ef9d36ea777194bc9b17f5f8678d2c56ee6a45b2c2f17eec96f242347"}, + {file = "astroid-2.15.4.tar.gz", hash = "sha256:c81e1c7fbac615037744d067a9bb5f9aeb655edf59b63ee8b59585475d6f80d8"}, ] [package.dependencies] @@ -99,54 +99,52 @@ uvloop = ["uvloop (>=0.15.2)"] [[package]] name = "casadi" -version = "3.6.1" +version = "3.5.5" description = "CasADi -- framework for algorithmic differentiation and numeric optimization" category = "main" optional = false python-versions = "*" files = [ - {file = "casadi-3.6.1-cp27-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:37086492e5e8fc54bbfad8cc50c952639ca5074d307c52389b27bafdcf62e057"}, - {file = "casadi-3.6.1-cp27-none-manylinux1_i686.whl", hash = "sha256:77b71f9bfaf16efbe85568537c891f9c21b9370cb9013280aa070c601c0429b9"}, - {file = "casadi-3.6.1-cp27-none-manylinux2010_x86_64.whl", hash = "sha256:5380bb03f42dea10c4a96c9e529d9ed13ecc49501621c1cf7a32550b9fce45d0"}, - {file = "casadi-3.6.1-cp27-none-win_amd64.whl", hash = "sha256:f94f2a4b68c4918062e483e14ec01e921e1967bbf5b6e535a68302cf6bea553c"}, - {file = "casadi-3.6.1-cp310-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:170c6971e90b1b74a291b2868130ce3658ed45698a81aed22f5110a2f0613477"}, - {file = "casadi-3.6.1-cp310-none-macosx_11_0_arm64.whl", hash = "sha256:83767449aa26f25a616c497a9bbc83ead776a8578493a7da3a228593f20e4298"}, - {file = "casadi-3.6.1-cp310-none-manylinux2014_i686.whl", hash = "sha256:d3d6e14c90c99d59fec7b5f00102dd0bc2653c1a750bb04fff277490616c9890"}, - {file = "casadi-3.6.1-cp310-none-manylinux2014_x86_64.whl", hash = "sha256:633799ddcaf09e3bd4fe2ff19f1c7d0e48316a965f44772a5cd8ec1a0cdfc06c"}, - {file = "casadi-3.6.1-cp310-none-win_amd64.whl", hash = "sha256:5977c0d36301d06f603330436db12abaae263a61e02fffc089c44b71bd64584f"}, - {file = "casadi-3.6.1-cp311-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:a77453a254a88a750dbdb4de5e5a415d6d788e517f6b3a2f7aec072b8811b467"}, - {file = "casadi-3.6.1-cp311-none-macosx_11_0_arm64.whl", hash = "sha256:e2ca945e0adc84d7b64a7ffed617b8e5c7fbc9098355fb805eeea664733cebde"}, - {file = "casadi-3.6.1-cp311-none-manylinux2014_i686.whl", hash = "sha256:2b88648c44596f97a8f655ec9f204f38d4294c5632e15c12324be6dc16c1e05a"}, - {file = "casadi-3.6.1-cp311-none-manylinux2014_x86_64.whl", hash = "sha256:f5f226dad0fbe87e497bfb3466e2d68d7c7190d0e0729a18688b95784e9246fa"}, - {file = "casadi-3.6.1-cp311-none-win_amd64.whl", hash = "sha256:3bc329e2c1679e7342130c02f092a2c16e9a4ed24c3694d0c479be996d60c611"}, - {file = "casadi-3.6.1-cp35-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:f2189529332cf43b5a49c23d87331e1f7a0d50db4f1346bbdc2db15565b5afdf"}, - {file = "casadi-3.6.1-cp35-none-manylinux1_i686.whl", hash = "sha256:48e89c1ad9cadda5a2760b0e72d22eba3958236c64ff6a83bbda00ceb8433557"}, - {file = "casadi-3.6.1-cp35-none-manylinux2010_x86_64.whl", hash = "sha256:a0e77f064e08d5b8dc3fa891eb9a11d56485ec75cdd409eb8c650a4e10b632a2"}, - {file = "casadi-3.6.1-cp35-none-win_amd64.whl", hash = "sha256:5a6bce4ecba2d21642d50dc5c559113d44af10854e2cda7c833f9e9fe4b106a2"}, - {file = "casadi-3.6.1-cp36-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:f6b00ffb2fb3701877d50c9d211bb0c4519159a612e717f780ee64ac71f8f372"}, - {file = "casadi-3.6.1-cp36-none-manylinux2014_i686.whl", hash = "sha256:69b62502b3ab650ddaf362db4494f476d456f98cc89873f7ec10957811b9bb20"}, - {file = "casadi-3.6.1-cp36-none-manylinux2014_x86_64.whl", hash = "sha256:eaba43c6aa82c79ca2aac6a52a9cce776d772a0e4453987fbdc8f89b19284f49"}, - {file = "casadi-3.6.1-cp36-none-win_amd64.whl", hash = "sha256:93f89716112638d8990e54feaea8804b550bc49d956b509e6357d59cbf77d61a"}, - {file = "casadi-3.6.1-cp37-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:df0c8de8a89671c473ee36c05c94c06ce74e3674cf35e3a5ebabf071f58afb69"}, - {file = "casadi-3.6.1-cp37-none-manylinux2014_i686.whl", hash = "sha256:0a786b1f2721ca5b3d5bdb8f6a829e932888598878be61e2980228f8151a1c18"}, - {file = "casadi-3.6.1-cp37-none-manylinux2014_x86_64.whl", hash = "sha256:1c5d5fbed5f80b83aade639cffc6504c88c400a502dc717f9da3b19d2e3ec3a8"}, - {file = "casadi-3.6.1-cp37-none-win_amd64.whl", hash = "sha256:42f83b7adc78a774a46a5972d481a066928679280d4c55bef160080916e95f6b"}, - {file = "casadi-3.6.1-cp38-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:d5f017ac4227116ce34d0a4883b276c63c0915aed53f67624f3989644ef22ff9"}, - {file = "casadi-3.6.1-cp38-none-macosx_11_0_arm64.whl", hash = "sha256:f37d7ab77378d31b7be3c3e1c313bf96dcc37e2ceb29a13e6c92fc7a31c496f4"}, - {file = "casadi-3.6.1-cp38-none-manylinux2014_i686.whl", hash = "sha256:4553d77b7c61b2bd7fdb650d901f77a37d40466cb0592643a398affcc97601e5"}, - {file = "casadi-3.6.1-cp38-none-manylinux2014_x86_64.whl", hash = "sha256:3257d7f2d0f8ebb5039b051e41bfb450226aa7e13476c0997f8bf56a611abde7"}, - {file = "casadi-3.6.1-cp38-none-win_amd64.whl", hash = "sha256:86929283540a688ab9b4351e8380f6978d4147f72ae3f882acc4541540e958ab"}, - {file = "casadi-3.6.1-cp39-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:f7fc0a6131db789821270a18cad81c9eed4ac185f0678c125d979885da6d354f"}, - {file = "casadi-3.6.1-cp39-none-macosx_11_0_arm64.whl", hash = "sha256:9d006d0d4da529967c71b45bc5b4b093ee3fd1629c85343a2c5596a674343605"}, - {file = "casadi-3.6.1-cp39-none-manylinux2014_i686.whl", hash = "sha256:b57e09977d09a07fa90dd5e8fc107bf95aa899c308dbccbd725b29f4ea2d0bb9"}, - {file = "casadi-3.6.1-cp39-none-manylinux2014_x86_64.whl", hash = "sha256:d8d7e184fc71979e1e7fec02a6c460155f001bb06b06d5397039eea05b14bcfe"}, - {file = "casadi-3.6.1-cp39-none-win_amd64.whl", hash = "sha256:c386cd6aa331b7fa7c455d8bf1bcd6b213913007bfc76793e596563c97a0cc50"}, - {file = "casadi-3.6.1.tar.gz", hash = "sha256:8566e23722bb10c8e1c0b08f87f9d22c6dc970181ee86aaf508ae0fd4b155b90"}, + {file = "casadi-3.5.5-cp27-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:d4e49cb46404cef61f83b30bb20ec9597c50ae7f55cfd6b89c17facc74675437"}, + {file = "casadi-3.5.5-cp27-none-manylinux1_i686.whl", hash = "sha256:f08a99e98b0a15083f06b1e221f064a29b3ed9e20617dc55aa8e823f2f732ace"}, + {file = "casadi-3.5.5-cp27-none-manylinux1_x86_64.whl", hash = "sha256:09e103bb597d46aa338fc57bc49270068a1f07be35f9494c9f796dea4b801aeb"}, + {file = "casadi-3.5.5-cp27-none-win32.whl", hash = "sha256:a4ce51e988570160af9ccfbbb1b9679546cbb1865d3a74ef0276f37fd94d91d9"}, + {file = "casadi-3.5.5-cp27-none-win_amd64.whl", hash = "sha256:54d89442058271007ae8573dfa33360bea10e26603545481090b45e8b90c9d10"}, + {file = "casadi-3.5.5-cp34-none-manylinux1_x86_64.whl", hash = "sha256:4143803af909f284400c02f59de4d97e5ba9319de28366215ef55ef261914f9a"}, + {file = "casadi-3.5.5-cp34-none-win32.whl", hash = "sha256:7a624d40c7b5ded7916f6cc65998af4585b4557c9ea65dc1e3a6273ebb2313ec"}, + {file = "casadi-3.5.5-cp34-none-win_amd64.whl", hash = "sha256:3aec6737c282e7fb5be41f6c7d0649e52ce49efb3508f30bada707e809bbbb5f"}, + {file = "casadi-3.5.5-cp35-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:6192e2ed81c15a7dab2554f5f69b134df8d1a982f8d9f13e57bdef93364d2120"}, + {file = "casadi-3.5.5-cp35-none-manylinux1_i686.whl", hash = "sha256:49a8b713f0ff0bbc2f2af2e71c515cdced238786e25ef504f5982618c84c67a7"}, + {file = "casadi-3.5.5-cp35-none-manylinux1_x86_64.whl", hash = "sha256:13277151efc76b221de8ca6b5ab7b8bbdd2b0e139f282866840adf88dfe53bc9"}, + {file = "casadi-3.5.5-cp35-none-manylinux2014_aarch64.whl", hash = "sha256:253569c85f881a6a8fe5e1c0758858edb1ecb4c3d8bce4aee4b52e5dc59fc091"}, + {file = "casadi-3.5.5-cp35-none-win32.whl", hash = "sha256:5de5c3c1381ac303e71fdef75dace34af6e1d50b46ac081051cd209b8b933837"}, + {file = "casadi-3.5.5-cp35-none-win_amd64.whl", hash = "sha256:4932b2b5361013420189dbc8d30e970672d036b37cb382f1c09c3b6cfe651a37"}, + {file = "casadi-3.5.5-cp36-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:2322748a8d5e88750fd2fc0abcdc56cfbad1a8cd538fe0e7d7b6d8ce0cb3fa62"}, + {file = "casadi-3.5.5-cp36-none-manylinux1_i686.whl", hash = "sha256:ab6a600a9b2ea27453d56fd4464ad0db0ae69f5cea42595fcbdaabcd40396440"}, + {file = "casadi-3.5.5-cp36-none-manylinux1_x86_64.whl", hash = "sha256:5f6eb8de31735c14ecc777e3ad77b57767b5f2dbea29265909ef696f51e8be92"}, + {file = "casadi-3.5.5-cp36-none-manylinux2014_aarch64.whl", hash = "sha256:adf20c34ba2cec1840a026023d93cc6d9b3581dfda6a044f434fc75b50c9a2ce"}, + {file = "casadi-3.5.5-cp36-none-win32.whl", hash = "sha256:7309a75b27c57f09b00a61815fb38c40da8e62e3004598e55ea1b8f713d96221"}, + {file = "casadi-3.5.5-cp36-none-win_amd64.whl", hash = "sha256:ab85c7cf772ba54f2718ebe366b836fffff868443f7c0c02389ed0a288cbde1f"}, + {file = "casadi-3.5.5-cp37-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:ec26244f9d9047f1bb401f1b86ff4775e1ddf638f4b4992bbc362a27a6f56673"}, + {file = "casadi-3.5.5-cp37-none-manylinux1_i686.whl", hash = "sha256:1c451a07b2440c00d552e040b6285b6e79b677d2978212368b28b86f5d267669"}, + {file = "casadi-3.5.5-cp37-none-manylinux1_x86_64.whl", hash = "sha256:24fbac649ee26572884029dcd0e108b4a2412cad003a84ed915c4e44a94ecae7"}, + {file = "casadi-3.5.5-cp37-none-manylinux2014_aarch64.whl", hash = "sha256:a06c0b96eb9d3bc88c627eec6e465726934ca0394347dc33efc742b8c91db83d"}, + {file = "casadi-3.5.5-cp37-none-win32.whl", hash = "sha256:36db4c84d8f3aad328faaeaeaa454a633c95a854d78ea188791b147888379342"}, + {file = "casadi-3.5.5-cp37-none-win_amd64.whl", hash = "sha256:643e48f92eaf65eb82964816bb7e7064ddb8239959210fa6168e8bce6fe6ef94"}, + {file = "casadi-3.5.5-cp38-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:6ce7ac8a301a145f98d46db0bfd13bc8b3831a5bb92e8054d531a1f233bb4b93"}, + {file = "casadi-3.5.5-cp38-none-manylinux1_i686.whl", hash = "sha256:473bb86fa64ac9703d74a474514703b4665fa9a384221ced620b5025e64532a7"}, + {file = "casadi-3.5.5-cp38-none-manylinux1_x86_64.whl", hash = "sha256:292e2768280393bad406256e0ef9c30ddcd4867dbd42148b36f9d92a32d9e199"}, + {file = "casadi-3.5.5-cp38-none-manylinux2014_aarch64.whl", hash = "sha256:353a79e50aa84ac5e0d9f04bc3b2d78a2cc8edae3b842d757756449682778944"}, + {file = "casadi-3.5.5-cp38-none-win32.whl", hash = "sha256:77f33cb95be6a49b93d8d6b81f05193676ae09857699cedf8f1a14a4285d077e"}, + {file = "casadi-3.5.5-cp38-none-win_amd64.whl", hash = "sha256:fbf39dcd63f1d3b63c300fce59b7ea678bd5ea1d014e1e090a5226600a4132cb"}, + {file = "casadi-3.5.5-cp39-none-macosx_10_13_x86_64.macosx_10_13_intel.whl", hash = "sha256:4086669280b2335d235c664373db46dcd7f6485dba4663ce1944ea01753c5e8b"}, + {file = "casadi-3.5.5-cp39-none-manylinux1_i686.whl", hash = "sha256:c3440c90c31b61ae1df82f6c784643393f723354dc08013f9d5cedf25507c67c"}, + {file = "casadi-3.5.5-cp39-none-manylinux1_x86_64.whl", hash = "sha256:bd94048388b602fc30fdac2fecb986c034110ed8d2d17af7fd13b0de45c58bd7"}, + {file = "casadi-3.5.5-cp39-none-manylinux2014_aarch64.whl", hash = "sha256:cd630a2e6ec6df6a4977af63080fa8d63a0053ff8c06ea0200959b47ae75201c"}, + {file = "casadi-3.5.5-cp39-none-win32.whl", hash = "sha256:ac45b91616e9b8afbe266ca08e80770b28e9e6d7a5852e3677fb37e42bde2047"}, + {file = "casadi-3.5.5-cp39-none-win_amd64.whl", hash = "sha256:55df534d003efdd120c4ebfeb6b252c443d273cdc4b97a394eb0268367477795"}, ] -[package.dependencies] -numpy = "*" - [[package]] name = "certifi" version = "2022.12.7" @@ -275,7 +273,7 @@ colorama = {version = "*", markers = "platform_system == \"Windows\""} name = "cloudpickle" version = "2.2.1" description = "Extended pickling support for Python objects" -category = "dev" +category = "main" optional = false python-versions = ">=3.6" files = [ @@ -347,7 +345,7 @@ development = ["black", "flake8", "mypy", "pytest", "types-colorama"] name = "deprecation" version = "2.1.0" description = "A library to handle automated deprecations" -category = "dev" +category = "main" optional = false python-versions = "*" files = [ @@ -515,7 +513,7 @@ files = [ name = "gym" version = "0.22.0" description = "Gym: A universal API for reinforcement learning environments." -category = "dev" +category = "main" optional = false python-versions = ">=3.7" files = [ @@ -543,7 +541,7 @@ toy-text = ["pygame (==2.1.0)", "scipy (>=1.4.1)"] name = "gym-notices" version = "0.0.8" description = "Notices for gym" -category = "dev" +category = "main" optional = false python-versions = "*" files = [ @@ -583,14 +581,14 @@ files = [ [[package]] name = "imageio" -version = "2.27.0" +version = "2.28.0" description = "Library for reading and writing a wide range of image, video, scientific, and volumetric data formats." category = "main" optional = false python-versions = ">=3.7" files = [ - {file = "imageio-2.27.0-py3-none-any.whl", hash = "sha256:24c6ad7d000e64eacc2861c402b6fb128f370cb0a6623cf796d83bca0d0d14d3"}, - {file = "imageio-2.27.0.tar.gz", hash = "sha256:ee269c957785ef0373cc7a7323185956d83ec05e6cdf20b42a03ba7b74ac58c6"}, + {file = "imageio-2.28.0-py3-none-any.whl", hash = "sha256:889d9eca66ac9f664d480b78447db1774e7f386bd3b0a06be7e32362d5390f4c"}, + {file = "imageio-2.28.0.tar.gz", hash = "sha256:467763bdb6e614d1ea8b4565e42c53fb2eb2fd4307a4601e97102a958c09812a"}, ] [package.dependencies] @@ -615,14 +613,14 @@ tifffile = ["tifffile"] [[package]] name = "importlib-metadata" -version = "6.5.0" +version = "6.6.0" description = "Read metadata from Python packages" category = "main" optional = false python-versions = ">=3.7" files = [ - {file = "importlib_metadata-6.5.0-py3-none-any.whl", hash = "sha256:03ba783c3a2c69d751b109fc0c94a62c51f581b3d6acf8ed1331b6d5729321ff"}, - {file = "importlib_metadata-6.5.0.tar.gz", hash = "sha256:7a8bdf1bc3a726297f5cfbc999e6e7ff6b4fa41b26bba4afc580448624460045"}, + {file = "importlib_metadata-6.6.0-py3-none-any.whl", hash = "sha256:43dd286a2cd8995d5eaef7fee2066340423b818ed3fd70adf0bad5f1fac53fed"}, + {file = "importlib_metadata-6.6.0.tar.gz", hash = "sha256:92501cdf9cc66ebd3e612f1b4f0c0765dfa42f0fa38ffb319b6bd84dd675d705"}, ] [package.dependencies] @@ -813,13 +811,13 @@ files = [ [[package]] name = "lit" -version = "16.0.1" +version = "16.0.2" description = "A Software Testing Tool" category = "main" optional = false python-versions = "*" files = [ - {file = "lit-16.0.1.tar.gz", hash = "sha256:630a47291b714cb115015df23ab04267c24fe59aec7ecd7e637d5c75cdb45c91"}, + {file = "lit-16.0.2.tar.gz", hash = "sha256:d743ef55cb58764bba85768c502e2d68d87aeb4303d508a18abaa8a35077ab25"}, ] [[package]] @@ -1248,40 +1246,34 @@ setuptools = "*" [[package]] name = "numpy" -version = "1.23.5" +version = "1.22.4" description = "NumPy is the fundamental package for array computing with Python." category = "main" optional = false python-versions = ">=3.8" files = [ - {file = "numpy-1.23.5-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:9c88793f78fca17da0145455f0d7826bcb9f37da4764af27ac945488116efe63"}, - {file = "numpy-1.23.5-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:e9f4c4e51567b616be64e05d517c79a8a22f3606499941d97bb76f2ca59f982d"}, - {file = "numpy-1.23.5-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:7903ba8ab592b82014713c491f6c5d3a1cde5b4a3bf116404e08f5b52f6daf43"}, - {file = "numpy-1.23.5-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:5e05b1c973a9f858c74367553e236f287e749465f773328c8ef31abe18f691e1"}, - {file = "numpy-1.23.5-cp310-cp310-win32.whl", hash = "sha256:522e26bbf6377e4d76403826ed689c295b0b238f46c28a7251ab94716da0b280"}, - {file = "numpy-1.23.5-cp310-cp310-win_amd64.whl", hash = "sha256:dbee87b469018961d1ad79b1a5d50c0ae850000b639bcb1b694e9981083243b6"}, - {file = "numpy-1.23.5-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:ce571367b6dfe60af04e04a1834ca2dc5f46004ac1cc756fb95319f64c095a96"}, - {file = "numpy-1.23.5-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:56e454c7833e94ec9769fa0f86e6ff8e42ee38ce0ce1fa4cbb747ea7e06d56aa"}, - {file = "numpy-1.23.5-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:5039f55555e1eab31124a5768898c9e22c25a65c1e0037f4d7c495a45778c9f2"}, - {file = "numpy-1.23.5-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:58f545efd1108e647604a1b5aa809591ccd2540f468a880bedb97247e72db387"}, - {file = "numpy-1.23.5-cp311-cp311-win32.whl", hash = "sha256:b2a9ab7c279c91974f756c84c365a669a887efa287365a8e2c418f8b3ba73fb0"}, - {file = "numpy-1.23.5-cp311-cp311-win_amd64.whl", hash = "sha256:0cbe9848fad08baf71de1a39e12d1b6310f1d5b2d0ea4de051058e6e1076852d"}, - {file = "numpy-1.23.5-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:f063b69b090c9d918f9df0a12116029e274daf0181df392839661c4c7ec9018a"}, - {file = "numpy-1.23.5-cp38-cp38-macosx_11_0_arm64.whl", hash = "sha256:0aaee12d8883552fadfc41e96b4c82ee7d794949e2a7c3b3a7201e968c7ecab9"}, - {file = "numpy-1.23.5-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:92c8c1e89a1f5028a4c6d9e3ccbe311b6ba53694811269b992c0b224269e2398"}, - {file = "numpy-1.23.5-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:d208a0f8729f3fb790ed18a003f3a57895b989b40ea4dce4717e9cf4af62c6bb"}, - {file = "numpy-1.23.5-cp38-cp38-win32.whl", hash = "sha256:06005a2ef6014e9956c09ba07654f9837d9e26696a0470e42beedadb78c11b07"}, - {file = "numpy-1.23.5-cp38-cp38-win_amd64.whl", hash = "sha256:ca51fcfcc5f9354c45f400059e88bc09215fb71a48d3768fb80e357f3b457e1e"}, - {file = "numpy-1.23.5-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:8969bfd28e85c81f3f94eb4a66bc2cf1dbdc5c18efc320af34bffc54d6b1e38f"}, - {file = "numpy-1.23.5-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:a7ac231a08bb37f852849bbb387a20a57574a97cfc7b6cabb488a4fc8be176de"}, - {file = "numpy-1.23.5-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:bf837dc63ba5c06dc8797c398db1e223a466c7ece27a1f7b5232ba3466aafe3d"}, - {file = "numpy-1.23.5-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:33161613d2269025873025b33e879825ec7b1d831317e68f4f2f0f84ed14c719"}, - {file = "numpy-1.23.5-cp39-cp39-win32.whl", hash = "sha256:af1da88f6bc3d2338ebbf0e22fe487821ea4d8e89053e25fa59d1d79786e7481"}, - {file = "numpy-1.23.5-cp39-cp39-win_amd64.whl", hash = "sha256:09b7847f7e83ca37c6e627682f145856de331049013853f344f37b0c9690e3df"}, - {file = "numpy-1.23.5-pp38-pypy38_pp73-macosx_10_9_x86_64.whl", hash = "sha256:abdde9f795cf292fb9651ed48185503a2ff29be87770c3b8e2a14b0cd7aa16f8"}, - {file = "numpy-1.23.5-pp38-pypy38_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:f9a909a8bae284d46bbfdefbdd4a262ba19d3bc9921b1e76126b1d21c3c34135"}, - {file = "numpy-1.23.5-pp38-pypy38_pp73-win_amd64.whl", hash = "sha256:01dd17cbb340bf0fc23981e52e1d18a9d4050792e8fb8363cecbf066a84b827d"}, - {file = "numpy-1.23.5.tar.gz", hash = "sha256:1b1766d6f397c18153d40015ddfc79ddb715cabadc04d2d228d4e5a8bc4ded1a"}, + {file = "numpy-1.22.4-cp310-cp310-macosx_10_14_x86_64.whl", hash = "sha256:ba9ead61dfb5d971d77b6c131a9dbee62294a932bf6a356e48c75ae684e635b3"}, + {file = "numpy-1.22.4-cp310-cp310-macosx_10_15_x86_64.whl", hash = "sha256:1ce7ab2053e36c0a71e7a13a7475bd3b1f54750b4b433adc96313e127b870887"}, + {file = "numpy-1.22.4-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:7228ad13744f63575b3a972d7ee4fd61815b2879998e70930d4ccf9ec721dce0"}, + {file = "numpy-1.22.4-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:43a8ca7391b626b4c4fe20aefe79fec683279e31e7c79716863b4b25021e0e74"}, + {file = "numpy-1.22.4-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:a911e317e8c826ea632205e63ed8507e0dc877dcdc49744584dfc363df9ca08c"}, + {file = "numpy-1.22.4-cp310-cp310-win32.whl", hash = "sha256:9ce7df0abeabe7fbd8ccbf343dc0db72f68549856b863ae3dd580255d009648e"}, + {file = "numpy-1.22.4-cp310-cp310-win_amd64.whl", hash = "sha256:3e1ffa4748168e1cc8d3cde93f006fe92b5421396221a02f2274aab6ac83b077"}, + {file = "numpy-1.22.4-cp38-cp38-macosx_10_15_x86_64.whl", hash = "sha256:59d55e634968b8f77d3fd674a3cf0b96e85147cd6556ec64ade018f27e9479e1"}, + {file = "numpy-1.22.4-cp38-cp38-macosx_11_0_arm64.whl", hash = "sha256:c1d937820db6e43bec43e8d016b9b3165dcb42892ea9f106c70fb13d430ffe72"}, + {file = "numpy-1.22.4-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:d4c5d5eb2ec8da0b4f50c9a843393971f31f1d60be87e0fb0917a49133d257d6"}, + {file = "numpy-1.22.4-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:64f56fc53a2d18b1924abd15745e30d82a5782b2cab3429aceecc6875bd5add0"}, + {file = "numpy-1.22.4-cp38-cp38-win32.whl", hash = "sha256:fb7a980c81dd932381f8228a426df8aeb70d59bbcda2af075b627bbc50207cba"}, + {file = "numpy-1.22.4-cp38-cp38-win_amd64.whl", hash = "sha256:e96d7f3096a36c8754207ab89d4b3282ba7b49ea140e4973591852c77d09eb76"}, + {file = "numpy-1.22.4-cp39-cp39-macosx_10_14_x86_64.whl", hash = "sha256:4c6036521f11a731ce0648f10c18ae66d7143865f19f7299943c985cdc95afb5"}, + {file = "numpy-1.22.4-cp39-cp39-macosx_10_15_x86_64.whl", hash = "sha256:b89bf9b94b3d624e7bb480344e91f68c1c6c75f026ed6755955117de00917a7c"}, + {file = "numpy-1.22.4-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:2d487e06ecbf1dc2f18e7efce82ded4f705f4bd0cd02677ffccfb39e5c284c7e"}, + {file = "numpy-1.22.4-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:f3eb268dbd5cfaffd9448113539e44e2dd1c5ca9ce25576f7c04a5453edc26fa"}, + {file = "numpy-1.22.4-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:37431a77ceb9307c28382c9773da9f306435135fae6b80b62a11c53cfedd8802"}, + {file = "numpy-1.22.4-cp39-cp39-win32.whl", hash = "sha256:cc7f00008eb7d3f2489fca6f334ec19ca63e31371be28fd5dad955b16ec285bd"}, + {file = "numpy-1.22.4-cp39-cp39-win_amd64.whl", hash = "sha256:f0725df166cf4785c0bc4cbfb320203182b1ecd30fee6e541c8752a92df6aa32"}, + {file = "numpy-1.22.4-pp38-pypy38_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:0791fbd1e43bf74b3502133207e378901272f3c156c4df4954cad833b1380207"}, + {file = "numpy-1.22.4.zip", hash = "sha256:425b390e4619f58d8526b3dcf656dde069133ae5c240229821f01b5f44ea07af"}, ] [[package]] @@ -1596,21 +1588,42 @@ files = [ {file = "pkgutil_resolve_name-1.3.10.tar.gz", hash = "sha256:357d6c9e6a755653cfd78893817c0853af365dd51ec97f3d358a819373bbd174"}, ] +[[package]] +name = "plannerbenchmark" +version = "1.1.1" +description = "Benchmark suite for local planner in dynamic environments. Multiple planners can be compared on different kinematic chains in different environment. The suite is highly extendable for new planners, robots and environments." +category = "dev" +optional = false +python-versions = ">=3.8,<3.10" +files = [ + {file = "plannerbenchmark-1.1.1-py3-none-any.whl", hash = "sha256:5670f2506360c2cc5df573cbf4f2b7d1bf74e9b525433c3855562fd90ce15bec"}, + {file = "plannerbenchmark-1.1.1.tar.gz", hash = "sha256:4d9f8e5da466481eb1be8f1601eb711eecd1ba80c2d2915cde2446dde46d5d60"}, +] + +[package.dependencies] +forwardkinematics = ">=1.1.1,<2.0.0" +robotmpcs = {version = ">=0.2.1,<0.3.0", optional = true, markers = "extra == \"mpc\""} +urdfenvs = ">=0.7.1,<0.8.0" + +[package.extras] +fabric = ["fabrics (>=0.6.2,<0.7.0)"] +mpc = ["robotmpcs (>=0.2.1,<0.3.0)"] + [[package]] name = "platformdirs" -version = "3.2.0" +version = "3.3.0" description = "A small Python package for determining appropriate platform-specific dirs, e.g. a \"user data dir\"." category = "dev" optional = false python-versions = ">=3.7" files = [ - {file = "platformdirs-3.2.0-py3-none-any.whl", hash = "sha256:ebe11c0d7a805086e99506aa331612429a72ca7cd52a1f0d277dc4adc20cb10e"}, - {file = "platformdirs-3.2.0.tar.gz", hash = "sha256:d5b638ca397f25f979350ff789db335903d7ea010ab28903f57b27e1b16c2b08"}, + {file = "platformdirs-3.3.0-py3-none-any.whl", hash = "sha256:ea61fd7b85554beecbbd3e9b37fb26689b227ffae38f73353cbcc1cf8bd01878"}, + {file = "platformdirs-3.3.0.tar.gz", hash = "sha256:64370d47dc3fca65b4879f89bdead8197e93e05d696d6d1816243ebae8595da5"}, ] [package.extras] -docs = ["furo (>=2022.12.7)", "proselint (>=0.13)", "sphinx (>=6.1.3)", "sphinx-autodoc-typehints (>=1.22,!=1.23.4)"] -test = ["appdirs (==1.4.4)", "covdefaults (>=2.3)", "pytest (>=7.2.2)", "pytest-cov (>=4)", "pytest-mock (>=3.10)"] +docs = ["furo (>=2023.3.27)", "proselint (>=0.13)", "sphinx (>=6.1.3)", "sphinx-autodoc-typehints (>=1.23,!=1.23.4)"] +test = ["appdirs (==1.4.4)", "covdefaults (>=2.3)", "pytest (>=7.3.1)", "pytest-cov (>=4)", "pytest-mock (>=3.10)"] [[package]] name = "pluggy" @@ -1648,11 +1661,26 @@ docs = ["sphinx (>=1.7.1)"] redis = ["redis"] tests = ["pytest (>=5.4.1)", "pytest-cov (>=2.8.1)", "pytest-mypy (>=0.8.0)", "pytest-timeout (>=2.1.0)", "redis", "sphinx (>=6.0.0)"] +[[package]] +name = "pyaml" +version = "21.10.1" +description = "PyYAML-based module to produce pretty and readable YAML-serialized data" +category = "dev" +optional = false +python-versions = "*" +files = [ + {file = "pyaml-21.10.1-py2.py3-none-any.whl", hash = "sha256:19985ed303c3a985de4cf8fd329b6d0a5a5b5c9035ea240eccc709ebacbaf4a0"}, + {file = "pyaml-21.10.1.tar.gz", hash = "sha256:c6519fee13bf06e3bb3f20cacdea8eba9140385a7c2546df5dbae4887f768383"}, +] + +[package.dependencies] +PyYAML = "*" + [[package]] name = "pybullet" version = "3.2.5" description = "Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning" -category = "dev" +category = "main" optional = false python-versions = "*" files = [ @@ -1686,18 +1714,18 @@ validation = ["lxml"] [[package]] name = "pylint" -version = "2.17.2" +version = "2.17.3" description = "python code static checker" category = "dev" optional = false python-versions = ">=3.7.2" files = [ - {file = "pylint-2.17.2-py3-none-any.whl", hash = "sha256:001cc91366a7df2970941d7e6bbefcbf98694e00102c1f121c531a814ddc2ea8"}, - {file = "pylint-2.17.2.tar.gz", hash = "sha256:1b647da5249e7c279118f657ca28b6aaebb299f86bf92affc632acf199f7adbb"}, + {file = "pylint-2.17.3-py3-none-any.whl", hash = "sha256:a6cbb4c6e96eab4a3c7de7c6383c512478f58f88d95764507d84c899d656a89a"}, + {file = "pylint-2.17.3.tar.gz", hash = "sha256:761907349e699f8afdcd56c4fe02f3021ab5b3a0fc26d19a9bfdc66c7d0d5cd5"}, ] [package.dependencies] -astroid = ">=2.15.2,<=2.17.0-dev0" +astroid = ">=2.15.4,<=2.17.0-dev0" colorama = {version = ">=0.4.5", markers = "sys_platform == \"win32\""} dill = {version = ">=0.2", markers = "python_version < \"3.11\""} isort = ">=4.2.5,<6" @@ -1948,6 +1976,30 @@ urllib3 = ">=1.21.1,<1.27" socks = ["PySocks (>=1.5.6,!=1.5.7)"] use-chardet-on-py3 = ["chardet (>=3.0.2,<6)"] +[[package]] +name = "robotmpcs" +version = "0.2.1" +description = "MPC generation for robots using ForcesPro." +category = "dev" +optional = false +python-versions = ">=3.8,<3.10" +files = [ + {file = "robotmpcs-0.2.1-py3-none-any.whl", hash = "sha256:647ffc5a724cd7102328b1a90907703d971a048d164f5fcbe4b5125b44472044"}, + {file = "robotmpcs-0.2.1.tar.gz", hash = "sha256:1fdfc510607eb320c514d177ccd4743aaa1bc77c96717d624544f88e39e01572"}, +] + +[package.dependencies] +casadi = ">=3.5.4,<3.5.5.post1 || >3.5.5.post1,<3.5.5.post2 || >3.5.5.post2,<4.0.0" +forwardkinematics = ">=1.1.1,<2.0.0" +numpy = "<1.23" +pyaml = ">=21.10.1,<22.0.0" +requests = ">=2.27.1,<3.0.0" +scipy = ">=1.5.0,<2.0.0" +setuptools = ">=67.5.1,<68.0.0" + +[package.extras] +agents = ["motion-planning-scenes (>=0.1,<0.2)", "planarenvs (>=1.0.3,<2.0.0)", "urdfenvs (>=0.2.2,<0.3.0)"] + [[package]] name = "rtree" version = "1.0.1" @@ -2044,14 +2096,14 @@ test = ["asv", "gmpy2", "mpmath", "pooch", "pytest", "pytest-cov", "pytest-timeo [[package]] name = "setuptools" -version = "67.7.0" +version = "67.7.2" description = "Easily download, build, install, upgrade, and uninstall Python packages" category = "main" optional = false python-versions = ">=3.7" files = [ - {file = "setuptools-67.7.0-py3-none-any.whl", hash = "sha256:888be97fde8cc3afd60f7784e678fa29ee13c4e5362daa7104a93bba33646c50"}, - {file = "setuptools-67.7.0.tar.gz", hash = "sha256:b7e53a01c6c654d26d2999ee033d8c6125e5fa55f03b7b193f937ae7ac999f22"}, + {file = "setuptools-67.7.2-py3-none-any.whl", hash = "sha256:23aaf86b85ca52ceb801d32703f12d77517b2556af839621c641fca11287952b"}, + {file = "setuptools-67.7.2.tar.gz", hash = "sha256:f104fa03692a2602fa0fec6c6a9e63b6c8a968de13e17c026957dd1f53d80990"}, ] [package.extras] @@ -2173,14 +2225,14 @@ widechars = ["wcwidth"] [[package]] name = "termcolor" -version = "2.2.0" +version = "2.3.0" description = "ANSI color formatting for output in terminal" category = "main" optional = false python-versions = ">=3.7" files = [ - {file = "termcolor-2.2.0-py3-none-any.whl", hash = "sha256:91ddd848e7251200eac969846cbae2dacd7d71c2871e92733289e7e3666f48e7"}, - {file = "termcolor-2.2.0.tar.gz", hash = "sha256:dfc8ac3f350788f23b2947b3e6cfa5a53b630b612e6cd8965a015a776020b99a"}, + {file = "termcolor-2.3.0-py3-none-any.whl", hash = "sha256:3afb05607b89aed0ffe25202399ee0867ad4d3cb4180d98aaf8eefa6a5f7d475"}, + {file = "termcolor-2.3.0.tar.gz", hash = "sha256:b5b08f68937f138fe92f6c089b99f1e2da0ae56c52b78bf7075fd95420fd9a5a"}, ] [package.extras] @@ -2429,14 +2481,14 @@ pyyaml = "*" [[package]] name = "urdfenvs" -version = "0.6.0" +version = "0.7.3" description = "Simple simulation environment for robots, based on the urdf files." -category = "dev" +category = "main" optional = false python-versions = ">=3.8,<4.0" files = [ - {file = "urdfenvs-0.6.0-py3-none-any.whl", hash = "sha256:25735046c1e08b988aa00a884f6fa8738bb9f790556518621ec9da2a2a9eab16"}, - {file = "urdfenvs-0.6.0.tar.gz", hash = "sha256:7bb68308b5107c9f2780abd626e2b9736dbf50320c1994e14ecbef47339b1a46"}, + {file = "urdfenvs-0.7.3-py3-none-any.whl", hash = "sha256:61c781150c0edfd0615f7d7dc5e61ade28fb0c889d4e30d2f0f595bbebf0ec5c"}, + {file = "urdfenvs-0.7.3.tar.gz", hash = "sha256:50ff650731884f21a8708382c91a9e12f13ec53ab6bc391cdff828d4a3c800ea"}, ] [package.dependencies] @@ -2728,4 +2780,4 @@ testing = ["big-O", "flake8 (<5)", "jaraco.functools", "jaraco.itertools", "more [metadata] lock-version = "2.0" python-versions = "^3.8,<3.9" -content-hash = "42e7a3e6a85071b1eb3589abe5372a40dcb1a7da53eeb223175b08ee6e5edb11" +content-hash = "77c80e9337bd40c06c18b6d1c07db15dcffdd70ee7b240eeb0a289d853fadabe" diff --git a/pyproject.toml b/pyproject.toml index f388798..82f6854 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -20,6 +20,8 @@ mpscenes = "^0.3.1" fabrics = "^0.6.2" yourdfpy = "^0.0.52" pytorch3d = "^0.3.0" +urdfenvs = "^0.7.1" +casadi = "3.5.5" [tool.poetry.group.dev] optional = true @@ -27,7 +29,12 @@ optional = true [tool.poetry.group.dev.dependencies] pylint = "^2.16.1" black = "^23.1.0" -urdfenvs = "^0.6.0" + +[tool.poetry.group.benchmark] +optional = true + +[tool.poetry.group.benchmark.dependencies] +plannerbenchmark = {version = "^1.1.1", extras = ["mpc"]} [build-system] requires = ["poetry-core"]