-
Notifications
You must be signed in to change notification settings - Fork 22
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #41 from tud-airlab/ft-benchmarks
Ft benchmarks
- Loading branch information
Showing
23 changed files
with
894 additions
and
124 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1 @@ | ||
jit_* |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1 @@ | ||
from .mppi_planner_wrapper import MPPIPlanner |
132 changes: 132 additions & 0 deletions
132
benchmarks/panda_arm/mppi_planner/mppi_planner_wrapper.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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() | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1 @@ | ||
*/* |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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' |
Oops, something went wrong.