From 0783d3e05bc0778f6b24ae4fed4453ebbcb04c9f Mon Sep 17 00:00:00 2001 From: Danfei Xu Date: Mon, 1 Feb 2021 14:35:35 -0800 Subject: [PATCH] Fix to cfg path to run from root perls2 directory. --- cfg/default_panda_controller.yaml | 4 ++-- demos/demo.py | 2 +- demos/joint_demo.py | 8 ++++---- examples/simple_reach/simple_reach_env.py | 2 -- examples/switch_sim_real_reach/run_switch_sim_real.py | 2 +- examples/switch_sim_real_reach/sim_real_reach.yaml | 11 +++++++++-- examples/switch_sim_real_reach/switch_sim_real_env.py | 7 +++---- 7 files changed, 20 insertions(+), 16 deletions(-) diff --git a/cfg/default_panda_controller.yaml b/cfg/default_panda_controller.yaml index 82bd82e3..d05aa169 100644 --- a/cfg/default_panda_controller.yaml +++ b/cfg/default_panda_controller.yaml @@ -8,8 +8,8 @@ panda_controller: damping: 1.0 #1 #0.5 #1 #0.5 # Damping factor [0,1] input_max: 1.0 input_min: -1.0 - output_max: 0.05 - output_min: -0.05 + output_max: 0.1 + output_min: -0.1 JointVelocity: kv: 10.0 #0.5 input_max: 1.0 diff --git a/demos/demo.py b/demos/demo.py index f428d338..b3f967b7 100644 --- a/demos/demo.py +++ b/demos/demo.py @@ -34,7 +34,7 @@ def __init__(self, ctrl_type, demo_type, test_fn, - config_file="demo_control_cfg.yaml", + config_file="demos/demo_control_cfg.yaml", **kwargs): self.ctrl_type = ctrl_type self.config_file = config_file diff --git a/demos/joint_demo.py b/demos/joint_demo.py index a3706740..cc6b7fa1 100644 --- a/demos/joint_demo.py +++ b/demos/joint_demo.py @@ -39,7 +39,7 @@ class JointSpaceDemo(Demo): def __init__(self, ctrl_type, demo_type, - config_file="demo_control_cfg.yaml", + config_file="demos/demo_control_cfg.yaml", delta_val=0.005, num_steps=30, test_fn='set_joint_delta', **kwargs): @@ -208,7 +208,7 @@ class SingleJointDemo(JointSpaceDemo): """Joint Space demonstration for a single joint. """ def __init__(self, - config_file="demo_control_cfg.yaml", + config_file="demos/demo_control_cfg.yaml", delta_val=0.005, num_steps=30, test_fn='set_joint_delta', joint_num=6, @@ -248,7 +248,7 @@ class GravityCompDemo(JointSpaceDemo): """Gravity Compensation to demonstrate "floating" arm. """ def __init__(self, - config_file="demo_control_cfg.yaml", + config_file="demos/demo_control_cfg.yaml", num_steps=100, **kwargs): @@ -280,7 +280,7 @@ def get_action(self, goal_state, current_state): class JointImpDemoSeq(SingleJointDemo): def __init__(self, - config_file="demo_control_cfg.yaml", + config_file="demos/demo_control_cfg.yaml", delta_val=0.005, num_steps=30, test_fn='set_joint_delta', **kwargs): diff --git a/examples/simple_reach/simple_reach_env.py b/examples/simple_reach/simple_reach_env.py index 47bccd49..c1e04f00 100644 --- a/examples/simple_reach/simple_reach_env.py +++ b/examples/simple_reach/simple_reach_env.py @@ -6,9 +6,7 @@ import math import numpy as np from perls2.utils.yaml_config import YamlConfig - from perls2.envs.env import Env - import gym.spaces as spaces import logging diff --git a/examples/switch_sim_real_reach/run_switch_sim_real.py b/examples/switch_sim_real_reach/run_switch_sim_real.py index 6cf32c84..d793b057 100644 --- a/examples/switch_sim_real_reach/run_switch_sim_real.py +++ b/examples/switch_sim_real_reach/run_switch_sim_real.py @@ -30,7 +30,7 @@ def get_action(observation): return action -env = SwitchSimRealEnv('./sim_real_reach.yaml', True, None) +env = SwitchSimRealEnv('examples/switch_sim_real_reach/sim_real_reach.yaml', True, None) # Lists for saving demonstrations training_list = [] diff --git a/examples/switch_sim_real_reach/sim_real_reach.yaml b/examples/switch_sim_real_reach/sim_real_reach.yaml index d960553e..3d6afcc1 100644 --- a/examples/switch_sim_real_reach/sim_real_reach.yaml +++ b/examples/switch_sim_real_reach/sim_real_reach.yaml @@ -1,13 +1,20 @@ # Cfg file for SimpleReachEnv world: type: 'Real' - robot: 'sawyer' + robot: 'panda' + controlType: 'EEImpedance' data_dir: '../../data' # Simulation parameters sim_params: time_step: 0.001 #0.004166 # 1./240. steps_per_action: 20 # policy to sim update ratio - MAX_STEPS: 10 + MAX_STEPS: 500 + +policy_freq: 20 # For gazebo? +control_freq: 500 # Hz + +# If set up has Kinect camera +real_camera: False # Robots are specified by types and urdf locations # also may include intial setup like poses and orientations diff --git a/examples/switch_sim_real_reach/switch_sim_real_env.py b/examples/switch_sim_real_reach/switch_sim_real_env.py index ee9ca938..fe5ad5df 100644 --- a/examples/switch_sim_real_reach/switch_sim_real_env.py +++ b/examples/switch_sim_real_reach/switch_sim_real_env.py @@ -1,6 +1,5 @@ import time import math -import pybullet import numpy as np from perls2.utils.yaml_config import YamlConfig import logging @@ -53,8 +52,8 @@ def __init__(self, self._initial_ee_orn = self.robot_interface.ee_orientation # For randomizing goal position in real world cases. - self.goal_upper_bound = np.add(self.reset_position, [0.1, 0.0, 0]) - self.goal_lower_bound = np.add(self.reset_position, [-0.1, -0.0, 0]) + self.goal_upper_bound = np.add(self.reset_position, [0.2, 0.0, 0]) + self.goal_lower_bound = np.add(self.reset_position, [0.1, -0.0, 0]) def reset(self): """Reset the environment. @@ -208,7 +207,7 @@ def _check_termination(self): print("Done - success!") return True if (self.num_steps > self.MAX_STEPS): - print("Done - max steps reached.") + print("Done - max steps reached.\n Final dist to goal \t{}".format(abs_dist)) return True else: return False