def __init__(self):
# We assume that a ROS node has already been created
# before initialising the environment
self.vel_pub = rospy.Publisher('/drone_1/cmd_vel', Twist, queue_size=5)
# self.takeoff_pub = rospy.Publisher('/drone/takeoff', EmptyTopicMsg, queue_size=0)
# gets training parameters from param server
self.speed_value = rospy.get_param("/speed_value")
self.desired_pose = Pose()
self.desired_pose.position.z = rospy.get_param("/desired_pose/z")
self.desired_pose.position.x = rospy.get_param("/desired_pose/x")
self.desired_pose.position.y = rospy.get_param("/desired_pose/y")
self.running_step = rospy.get_param("/running_step")
self.max_incl = rospy.get_param("/max_incl")
self.max_altitude = rospy.get_param("/max_altitude")
# stablishes connection with simulator
self.gazebo = GazeboConnection()
self.action_space = spaces.Discrete(5) #Forward,Left,Right,Up,Down
self.reward_range = (-np.inf, np.inf)
self._seed()
self.goal_pose = [1, 1, 1]
self.goal_threshold = 0.05
self.goal_reward = 50
self.prev_state = []
def launch_sim(self):
rospy.loginfo('LAUNCH SIM')
launch_gazebo_cmd = 'roslaunch crazyflie_gazebo crazyflie_sim.launch'
gazebo_process = subprocess.Popen(launch_gazebo_cmd,
stdout=subprocess.PIPE,
shell=True,
preexec_fn=os.setsid)
time.sleep(5)
self.gazebo = GazeboConnection()
cf_gazebo_path = '/home/brian/catkin_ws/src/sim_cf/crazyflie_gazebo/scripts'
launch_controller_cmd = './run_cfs.sh'
cf_process = subprocess.Popen(launch_controller_cmd,
stdout=subprocess.PIPE,
cwd=cf_gazebo_path,
shell=True,
preexec_fn=os.setsid)
self.gazebo.unpauseSim()
time.sleep(5)
self.first_reset = True
return gazebo_process, cf_process
def __init__(self):
# self.sess = tf.InteractiveSession()
# action publisher
self.cmd_pub = rospy.Publisher("/mobile_base/commands/velocity",
Twist,
queue_size=2)
self.running_step = 1 / 50.0
self.num_agents = 4
self.state_dim = state_dim
self.action_dim = 2
self.rate = rospy.Rate(40.0)
high = np.array([1. for _ in range(self.action_dim)])
low = np.array([0. for _ in range(self.action_dim)])
self.action_space = spaces.Box(low=low, high=high)
# stablishes connection with simulator
self.gazebo = GazeboConnection()
# state object
self.state_object = NavigationState(self.state_dim)
self.state_msg = ModelState()
self._seed()
print("end of init...")
self.gazebo.unpauseSim()
def __init__(self):
# specify the dimension of observation space and action space
self.observation_space = 44
self.action_space = 4 # joint efforts
# We assume that a ROS node has already been created before initialising the environment
self.running_step = rospy.get_param("/running_step")
self.weight_r1 = rospy.get_param("/weight_r1")
self.weight_r2 = rospy.get_param("/weight_r2")
self.weight_r3 = rospy.get_param("/weight_r3")
# self.inr_r3 = rospy.get_param("/inr_r3")
# stablishes connection with simulator
self.gazebo = GazeboConnection()
self.mrm_state_object = MrmState(weight_r1=self.weight_r1,
weight_r2=self.weight_r2,
weight_r3=self.weight_r3)
self.mrm_joint_pubisher_object = JointPub()
self._seed()
self.reset_controller_pub = rospy.Publisher('/reset_controller',
Bool,
queue_size=1)
self.pause_sim_pub = rospy.Publisher('/pause_sim', Bool, queue_size=1)
# list for recording the minimal value from the laser
self.mini_laser = []
self.moco = model_control()
self.pipe_angle = 0.0
def __init__(self):
# We assume that a ROS node has already been created
# before initialising the environment
self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
self.takeoff_pub = rospy.Publisher('/ardrone/takeoff', EmptyTopicMsg, queue_size=0)
# gets training parameters from param server
self.speed_value = rospy.get_param("/speed_value")
self.desired_pose = Pose()
self.desired_pose.position.z = rospy.get_param("/desired_pose/z")
self.desired_pose.position.x = rospy.get_param("/desired_pose/x")
self.desired_pose.position.y = rospy.get_param("/desired_pose/y")
self.running_step = rospy.get_param("/running_step")
self.max_incl = rospy.get_param("/max_incl")
self.max_altitude = rospy.get_param("/max_altitude")
self.max_distance = rospy.get_param("/max_distance")
# stablishes connection with simulator
self.gazebo = GazeboConnection()
high = np.array([
2000,
2000,
2000,
2000,
2000,
2000])
self.action_space = spaces.Discrete(6) #Forward,Left,Right,Up,Down,Back
self.observation_space = spaces.Box(-high, high)
self.reward_range = (-np.inf, np.inf)
self.seed()
def __init__(self):
self.publishers_array = []
self._base_pub = rospy.Publisher('/cart_pole_3d/cart_joint_velocity_controller/command', Float64, queue_size=1)
# self._pole_pub = rospy.Publisher('/cartpole_v0/pole_joint_velocity_controller/command', Float64, queue_size=1)
self.publishers_array.append(self._base_pub)
# self.publishers_array.append(self._pole_pub)
self.action_space = spaces.Discrete(2) #l,r,L,R,nothing
self._seed()
#get configuration parameters
self.min_pole_angle = rospy.get_param('/cart_pole_3d/min_angle')
self.max_pole_angle = rospy.get_param('/cart_pole_3d/max_angle')
self.max_base_velocity = rospy.get_param('/cart_pole_3d/cart_speed_fixed_value')
self.min_base_position = -rospy.get_param('/cart_pole_3d/max_distance')
self.max_base_position = rospy.get_param('/cart_pole_3d/max_distance')
self.pos_step = rospy.get_param('/cart_pole_3d/pos_step')
self.running_step = rospy.get_param('/cart_pole_3d/running_step')
self.init_pos = rospy.get_param('/cart_pole_3d/init_cart_vel')
self.wait_time = rospy.get_param('/cart_pole_3d/wait_time')
self.init_internal_vars(self.init_pos)
rospy.Subscriber("/cart_pole_3d/joint_states", JointState, self.joints_callback)
# stablishes connection with simulator
self.controllers_list = ['cart_joint_velocity_controller']
self.gazebo = GazeboConnection()
self.controllers_object = ControllersConnection(namespace="cart_pole_3d",
controllers_list=self.controllers_list)
def __init__(self):
# We assume that a ROS node has already been created
# before initialising the environment
self.position_pub = rospy.Publisher('/drone/command/pose',
PoseStamped,
queue_size=100)
# gets training parameters from param server
self.speed_value = rospy.get_param("/speed_value")
self.desired_pose = Pose()
self.desired_pose.position.z = rospy.get_param("/desired_pose/z")
self.desired_pose.position.x = rospy.get_param("/desired_pose/x")
self.desired_pose.position.y = rospy.get_param("/desired_pose/y")
self.running_step = rospy.get_param("/running_step")
self.max_incl = rospy.get_param("/max_incl")
self.max_altitude = rospy.get_param("/max_altitude")
# stablishes connection with simulator
self.gazebo = GazeboConnection()
self.action_space = spaces.Discrete(
5) #Forward,Backward,Left,Right,Still
self.reward_range = (-np.inf, np.inf)
self.seed()
def __init__(self):
self.publishers_array = []
self._roll_vel_pub = rospy.Publisher(
'/moving_cube/inertia_wheel_roll_joint_velocity_controller/command',
Float64,
queue_size=1)
self.publishers_array.append(self._roll_vel_pub)
self.action_space = spaces.Discrete(3) #l,r,nothing
self._seed()
#get configuration parameters
self.wait_time = rospy.get_param('/moving_cube/wait_time')
self.running_step = rospy.get_param('/moving_cube/running_step')
self.speed_step = rospy.get_param('/moving_cube/speed_step')
self.init_roll_vel = rospy.get_param('/moving_cube/init_roll_vel')
self.init_internal_vars(self.init_roll_vel)
rospy.Subscriber("/moving_cube/joint_states", JointState,
self.joints_callback)
rospy.Subscriber("/moving_cube/odom", Odometry, self.odom_callback)
# stablishes connection with simulator
self.gazebo = GazeboConnection()
self.controllers_object = ControllersConnection(
namespace="moving_cube")
def __init__(self):
# specify the dimension of observation space and action space
self.observation_space = 22
self.action_space = 4
# We assume that a ROS node has already been created before initialising the environment
self.running_step = rospy.get_param("/running_step")
self.done_reward = rospy.get_param("/collision_reward")
self.weight_r1 = rospy.get_param("/weight_r1")
self.weight_r2 = rospy.get_param("/weight_r2")
self.weight_r3 = rospy.get_param("/weight_r3")
# stablishes connection with simulator
self.gazebo = GazeboConnection()
self.controllers_object = ControllersConnection(namespace="")
self.mrm_state_object = MrmState(weight_r1=self.weight_r1,
weight_r2=self.weight_r2,
weight_r3=self.weight_r3)
self.mrm_joint_pubisher_object = JointPub()
self._seed()
self.reset_controller_pub = rospy.Publisher('/reset_controller',
Bool,
queue_size=1)
self.pause_sim_pub = rospy.Publisher('/pause_sim', Bool, queue_size=1)
def __init__(self,
ros_pkg_name,
launch_file,
start_init_physics_parameters=True,
reset_world_or_sim="SIMULATION"):
# To reset Simulations
rospy.logdebug("START init RosGazeboEnv")
self.gazebo = GazeboConnection(start_init_physics_parameters,
reset_world_or_sim)
self.seed()
# Set up ROS related variables
self.episode_num = 0
self.cumulated_episode_reward = 0
self.reward_pub = rospy.Publisher('/openai/reward',
RLExperimentInfo,
queue_size=1)
self.ros_launcher = ROSLauncher(rospackage_name=ros_pkg_name,\
launch_file_name=launch_file)
self.gazebo.unpauseSim()
self._setup_subscribers()
self._setup_publishers()
self._check_all_systems_ready()
self.gazebo.pauseSim()
rospy.logdebug("END init RosGazeboEnv and Paused Sim")
def __init__(self):
number_actions = rospy.get_param('/moving_cube/n_actions')
self.action_space = spaces.Discrete(number_actions)
self._seed()
#get configuration parameters
self.init_roll_vel = rospy.get_param('/moving_cube/init_roll_vel')
# Actions
self.roll_speed_fixed_value = rospy.get_param(
'/moving_cube/roll_speed_fixed_value')
self.roll_speed_increment_value = rospy.get_param(
'/moving_cube/roll_speed_increment_value')
self.start_point = Point()
self.start_point.x = rospy.get_param("/moving_cube/init_cube_pose/x")
self.start_point.y = rospy.get_param("/moving_cube/init_cube_pose/y")
self.start_point.z = rospy.get_param("/moving_cube/init_cube_pose/z")
# Done
self.max_pitch_angle = rospy.get_param('/moving_cube/max_pitch_angle')
# Rewards
self.move_distance_reward_weight = rospy.get_param(
"/moving_cube/move_distance_reward_weight")
self.y_linear_speed_reward_weight = rospy.get_param(
"/moving_cube/y_linear_speed_reward_weight")
self.y_axis_angle_reward_weight = rospy.get_param(
"/moving_cube/y_axis_angle_reward_weight")
self.end_episode_points = rospy.get_param(
"/moving_cube/end_episode_points")
# stablishes connection with simulator
self.gazebo = GazeboConnection()
self.controllers_list = [
'joint_state_controller',
'inertia_wheel_roll_joint_velocity_controller'
]
self.controllers_object = ControllersConnection(
namespace="moving_cube", controllers_list=self.controllers_list)
self.gazebo.unpauseSim()
self.controllers_object.reset_controllers()
self.check_all_sensors_ready()
rospy.Subscriber("/moving_cube/joint_states", JointState,
self.joints_callback)
rospy.Subscriber("/moving_cube/odom", Odometry, self.odom_callback)
self._roll_vel_pub = rospy.Publisher(
'/moving_cube/inertia_wheel_roll_joint_velocity_controller/command',
Float64,
queue_size=1)
self.check_publishers_connection()
self.gazebo.pauseSim()
def __init__(self):
# We assume that a ROS node has already been created
# before initialising the environment
self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
self.takeoff_pub = rospy.Publisher('/drone/takeoff',
EmptyTopicMsg,
queue_size=0)
self.gimbal_pub = rospy.Publisher('/dji_sdk/gimbal',
Gimbal,
queue_size=5)
# gets training parameters from param server
'''
self.speed_value = rospy.get_param("/speed_value")
self.desired_pose = Pose()
self.desired_pose.position.z = rospy.get_param("/desired_pose/z")
self.desired_pose.position.x = rospy.get_param("/desired_pose/x")
self.desired_pose.position.y = rospy.get_param("/desired_pose/y")
self.running_step = rospy.get_param("/running_step")
self.max_incl = rospy.get_param("/max_incl")
self.max_altitude = rospy.get_param("/max_altitude")
self.min_altitude = rospy.get_param("/min_altitude")
'''
self.speed_value = 1
self.desired_pose = Pose()
self.desired_pose.position.z = 0.7
self.desired_pose.position.x = 14
self.desired_pose.position.y = 1
self.running_step = 0.05
self.max_incl = 0.7
self.max_altitude = 20
self.min_altitude = 0.1
self.max_x = 15
self.max_y = 10
# stablishes connection with simulator
self.gazebo = GazeboConnection()
self.action_space = 2
self.observation_space = 6
self.reward_range = (-np.inf, np.inf)
self._seed()
self.shapestore = []
self.shapestore.append(0)
self.act = np.zeros(3)
self.pr = 0
self.pdlen = 0
self.plotpd = pd.DataFrame({
'p_x': [],
'p_y': [],
'v_x': [],
'v_y': [],
'time': []
})
self.tinit = time.time()
def __init__(self): # We initialize and define init function.
number_actions = rospy.get_param('/my_moving_cube/n_actions')
self.action_space = spaces.Discrete(number_actions)
self._seed()
# get configuration parameters
self.init_roll_vel = rospy.get_param('/my_moving_cube/init_roll_vel')
# Actions
self.roll_speed_fixed_value = rospy.get_param('/my_moving_cube/roll_speed_fixed_value')
self.roll_speed_increment_value = rospy.get_param('/my_moving_cube/roll_speed_increment_value')
self.start_point = Point()
self.start_point.x = rospy.get_param("/my_moving_cube/init_cube_pose/x")
self.start_point.y = rospy.get_param("/my_moving_cube/init_cube_pose/y")
self.start_point.z = rospy.get_param("/my_moving_cube/init_cube_pose/z")
# Done
self.max_pitch_angle = rospy.get_param('/my_moving_cube/max_pitch_angle')
# Rewards
self.move_distance_reward_weight = rospy.get_param("/my_moving_cube/move_distance_reward_weight")
self.y_linear_speed_reward_weight = rospy.get_param("/my_moving_cube/y_linear_speed_reward_weight")
self.y_axis_angle_reward_weight = rospy.get_param("/my_moving_cube/y_axis_angle_reward_weight")
self.end_episode_points = rospy.get_param("/my_moving_cube/end_episode_points")
# stablishes connection with simulator
self.gazebo = GazeboConnection()
self.controllers_list = ['joint_state_controller',
'inertia_wheel_roll_joint_velocity_controller'
]
self.controllers_object = ControllersConnection(namespace="my_moving_cube",
controllers_list=self.controllers_list)
"""
Namespace of robot is the namespace in front of your controller.
Controllers_list => that we want to reset at each time that we call the reset controllers.
This case we just have two: joint_state_controller, inertia_wheel_roll_joint_velocity_controller
We need to create a function which gets retrieves those controllers.
"""
self.gazebo.unpauseSim()
self.controllers_object.reset_controllers()
self.check_all_sensors_ready()
rospy.Subscriber("/my_moving_cube/joint_states", JointState, self.joints_callback)
rospy.Subscriber("/my_moving_cube/odom", Odometry, self.odom_callback)
self._roll_vel_pub = rospy.Publisher('/my_moving_cube/inertia_wheel_roll_joint_velocity_controller/command',
Float64, queue_size=1)
self.check_publishers_connection()
self.gazebo.pauseSim()
def __init__(self):
# We assume that a ROS node has already been created
# before initialising the environment
self.pos_pub = rospy.Publisher('/drone/cmd_pos', Point, queue_size=1)
self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
self.takeoff_pub = rospy.Publisher('/drone/takeoff',
EmptyTopicMsg,
queue_size=0)
self.switch_pub = rospy.Publisher('/drone/posctrl', Bool, queue_size=1)
# gets training parameters from param server
self.desired_pose = Pose()
self.desired_pose.position.z = rospy.get_param("/desired_pose/z")
self.desired_pose.position.x = rospy.get_param("/desired_pose/x")
self.desired_pose.position.y = rospy.get_param("/desired_pose/y")
self.running_step = rospy.get_param("/running_step")
self.max_incl = rospy.get_param("/max_incl")
self.minx = rospy.get_param("/limits/min_x")
self.maxx = rospy.get_param("/limits/max_x")
self.miny = rospy.get_param("/limits/min_y")
self.maxy = rospy.get_param("/limits/max_y")
#self.max_altitude = rospy.get_param("/limits/max_altitude")
#self.min_altitude = rospy.get_param("/limits/min_altitude")
self.shape = (10, 10)
self.incx = (self.maxx - self.minx) / (self.shape[0])
self.incy = (self.maxy - self.miny) / (self.shape[1])
#self.incz = (self.max_altitude - self.min_altitude)/ self.shape[0]
self.horizontal_bins = np.zeros((2, self.shape[1]))
#self.vertical_bin = np.zeros((self.shape[0]))
self.horizontal_bins[0] = np.linspace(self.minx, self.maxx,
self.shape[1])
self.horizontal_bins[1] = np.linspace(self.miny, self.maxy,
self.shape[1])
#self.vertical_bin = np.linspace(self.min_altitude,self.max_altitude,self.shape[0])
self.goal = np.zeros(2)
#self.goal[0] = int(np.digitize(self.desired_pose.position.z,self.vertical_bin))
self.goal[0] = int(
np.digitize(self.desired_pose.position.x, self.horizontal_bins[0]))
self.goal[1] = int(
np.digitize(self.desired_pose.position.y, self.horizontal_bins[1]))
rospy.loginfo("Goal: %s" % (self.goal))
# stablishes connection with simulator
self.gazebo = GazeboConnection()
self.action_space = spaces.Discrete(4) #Forward,Backward,Left,Right
self.reward_range = (-np.inf, np.inf)
self._seed()
self.init_desired_pose()
def __init__(self):
# before initialising the environment
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# stablishes connection with simulator
self.gazebo = GazeboConnection()
self.prev_pitch = 0
self.prev_time = 0
self.prev_accel = 0
random.seed(42)
def __init__(self):
# We assume that a ROS node has already been created
# before initialising the environment
self.pos_pub = rospy.Publisher('/drone/cmd_pos', Point, queue_size=1)
self.takeoff_pub = rospy.Publisher('/drone/takeoff',
EmptyTopicMsg,
queue_size=0)
self.switch_pub = rospy.Publisher('/drone/posctrl', Bool, queue_size=1)
self.del_model_client = rospy.ServiceProxy('gazebo/delete_model',
DeleteModel)
self.spawn_model_client = rospy.ServiceProxy('/gazebo/spawn_sdf_model',
SpawnModel)
self.running_step = rospy.get_param("/running_step")
self.minx = rospy.get_param("/limits/min_x")
self.maxx = rospy.get_param("/limits/max_x")
self.miny = rospy.get_param("/limits/min_y")
self.maxy = rospy.get_param("/limits/max_y")
self.base = Point()
self.base.x = rospy.get_param("/base/x")
self.base.y = rospy.get_param("/base/y")
self.base.z = rospy.get_param("/base/z")
self.gazebo = GazeboConnection()
# Get number of survivors
self.survivors = []
self.get_survivor_information()
self.rescued = np.full((len(self.survivors)), False)
# All of the survivors + return to base.
self.action_space = spaces.Discrete(len(self.survivors) + 1)
self.num_actions = len(self.survivors) + 1
self.reward_range = (-np.inf, np.inf)
self.battery = 100
self.battery_depletion = rospy.get_param("/battery_depletion")
self.battery_timer = rospy.Timer(rospy.Duration(2),
self.battery_timer_callback)
area_limits_high = np.array([
self.maxx, #x
self.maxy, #y
100 #battery
])
area_limits_low = np.array([self.minx, self.miny, 0])
self.observation_space = spaces.Box(area_limits_low, area_limits_high)
self._seed()
file_location = roslib.packages.get_pkg_dir(
'sjtu_drone') + '/models/person_standing/model.sdf'
file_xml = open(file_location)
self.person_standing = file_xml.read()
self.first_reset = True
def __init__(self, robot_name_space, controllers_list, reset_controls):
# To reset Simulations
print("Entered Gazebo Env")
self.gazebo = GazeboConnection(start_init_physics_parameters=False,
reset_world_or_sim="WORLD")
self.controllers_object = ControllersConnection(
namespace=robot_name_space, controllers_list=controllers_list)
self.reset_controls = reset_controls
print(self.reset_controls)
self.seed()
# Set up ROS related variables
self.episode_num = 0
self.reward_pub = rospy.Publisher('/openai/reward',
RLExperimentInfo,
queue_size=1)
def __init__(self, debug):
# We assume that a ROS node has already been created
# before initialising the environment
self.vel_pub = rospy.Publisher('/drone_1/cmd_vel', Twist, queue_size=5)
self.takeoff_pub = rospy.Publisher('/drone_1/takeoff',
EmptyTopicMsg,
queue_size=0)
self.pose_pub = rospy.Publisher('/drone_1/command/pose',
PoseStamped,
queue_size=5)
# gets training parameters from param server
self.speed_value = rospy.get_param("/speed_value")
self.desired_pose = Pose()
self.desired_pose.position.z = rospy.get_param("/desired_pose/z")
self.desired_pose.position.x = rospy.get_param("/desired_pose/x")
self.desired_pose.position.y = rospy.get_param("/desired_pose/y")
self.running_step = rospy.get_param("/running_step")
self.max_incl = rospy.get_param("/max_incl")
self.max_altitude = rospy.get_param("/max_altitude")
# stablishes connection with simulator
self.gazebo = GazeboConnection()
#self.action_space = spaces.Discrete(5) #Forward,Left,Right,Up,Down
self.num_states = 3
self.num_actions = 3
self.reward_range = (-np.inf, np.inf)
self._seed()
self.goal_pose = [5, -5, 5]
self.goal_threshold = 0.5
self.goal_reward = 50
self.prev_state = []
# Spatial limits
self.ceiling = 10
self.floor = 0.3
self.x_limit = 10
self.y_limit = 10
self.cmd_achieved = False
self.best_dist = 0
def __init__(self):
# specify the dimension of observation space and action space
self.observation_space = 44
self.action_space = 4 # joint efforts
# We assume that a ROS node has already been created before initialising the environment
# gets training parameters from param server
self.desired_target = Point()
self.desired_target.x = rospy.get_param("/desired_target_point/x")
self.desired_target.y = rospy.get_param("/desired_target_point/y")
self.desired_target.z = rospy.get_param("/desired_target_point/z")
self.running_step = rospy.get_param("/running_step")
self.weight_r1 = rospy.get_param("/weight_r1")
self.weight_r2 = rospy.get_param("/weight_r2")
self.weight_r3 = rospy.get_param("/weight_r3")
self.inr_r3 = rospy.get_param("/inr_r3")
# stablishes connection with simulator
self.gazebo = GazeboConnection()
self.mrm_state_object = MrmState(weight_r1=self.weight_r1,
weight_r2=self.weight_r2,
weight_r3=self.weight_r3)
self.mrm_state_object.set_desired_target_point(self.desired_target.x,
self.desired_target.y,
self.desired_target.z)
self.mrm_joint_pubisher_object = JointPub()
self._seed()
self.passed_episode = 0
self.total_episode = rospy.get_param("/total_episode")
# list for recording the minimal value from the laser
self.mini_laser = []
self.obstacle_penalty = 0.0
def __init__(self):
# Subscribe to the /ardrone/navdata topic, of message type navdata, and call self.ReceiveNavdata when a message is received
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata, self.receive_navdata)
self.subOdom = rospy.Subscriber('/ground_truth/state', Odometry, self.odom_callback)
rospy.Subscriber('/vrpn_client_node/TestTed/pose', PoseStamped, self.optictrack_callback)
# Allow the controller to publish to the /ardrone/takeoff, land and reset topics
self.pubLand = rospy.Publisher('/ardrone/land', Empty, queue_size=0)
self.pubTakeoff = rospy.Publisher('/ardrone/takeoff', Empty, queue_size=0)
self.pubReset = rospy.Publisher('/ardrone/reset', Empty, queue_size=1)
# Allow the controller to publish to the /cmd_vel topic and thus control the drone
self.pubCommand = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# Put location into odometry
self.location = Odometry()
self.status = Navdata()
self.real_loc = PoseStamped()
# Gets parameters from param server
self.speed_value = rospy.get_param("/speed_value")
self.run_step = rospy.get_param("/run_step")
self.desired_pose = Pose()
self.desired_pose.position.z = rospy.get_param("/desired_pose/z")
self.desired_pose.position.x = rospy.get_param("/desired_pose/x")
self.desired_pose.position.y = rospy.get_param("/desired_pose/y")
self.max_incl = rospy.get_param("/max_incl")
self.max_altitude = rospy.get_param("/max_altitude")
self.exporation_noise = OUNoise()
self.on_place = 0
# initialize action space
# Forward,Left,Right,Up,Down
#self.action_space = spaces.Discrete(8)
self.action_space = spaces.Box(np.array((-0.5, -0.5, -0.5, -0.5)), np.array((0.5, 0.5, 0.5, 0.5)))
# Gazebo Connection
self.gazebo = GazeboConnection()
self._seed()
# Land the drone if we are shutting down
rospy.on_shutdown(self.send_land)
def __init__(self):
self.running_step = 0.3
self.mode = ''
self.count = 0
self.current_init_pose = PoseStamped()
self.desired_pose = PoseStamped()
self.desired_pose.pose.position.x = 6
self.desired_pose.pose.position.y = 10
self.desired_pose.pose.position.z = 5
self.gazebo = GazeboConnection()
self.arming = armtakeoff()
self.data_pose = PoseStamped()
self.vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=20)
self.action_space = spaces.Discrete(5)
self.reward_range = (-np.inf, np.inf)
self.seed()
def __init__(self,
robot_name_space,
controllers_list,
reset_controls,
start_init_physics_parameters=True,
reset_world_or_sim="SIMULATION"):
# To reset Simulations
rospy.logdebug("START init RobotGazeboEnv")
self.gazebo = GazeboConnection(start_init_physics_parameters,
reset_world_or_sim)
self.controllers_object = ControllersConnection(
namespace=robot_name_space, controllers_list=controllers_list)
self.reset_controls = reset_controls
self.seed()
# Set up ROS related variables
self.episode_num = 0
self.cumulated_episode_reward = 0
self.reward_pub = rospy.Publisher('/openai/reward',
RLExperimentInfo,
queue_size=1)
rospy.logdebug("END init RobotGazeboEnv")
def __init__(self):
# os.system('rosparam load /home/lucasyu/catkin_ws/src/collaborative_transportation/rotors_gazebo/scripts/collaborative/MARL/config/MADDPG_params.yaml')
# We assume that a ROS node has already been created
# before initialising the environment
self.set_link = rospy.ServiceProxy('/gazebo/set_link_state',
SetLinkState)
rospy.wait_for_service('/gazebo/set_link_state')
# gets training parameters from param server
self.desired_pose = Pose()
self.desired_pose.position.x = rospy.get_param(
"/desired_pose/position/x")
self.desired_pose.position.y = rospy.get_param(
"/desired_pose/position/y")
self.desired_pose.position.z = rospy.get_param(
"/desired_pose/position/z")
self.desired_pose.orientation.x = rospy.get_param(
"/desired_pose/orientation/x")
self.desired_pose.orientation.y = rospy.get_param(
"/desired_pose/orientation/y")
self.desired_pose.orientation.z = rospy.get_param(
"/desired_pose/orientation/z")
self.running_step = rospy.get_param("/running_step")
self.max_incl = rospy.get_param("/max_incl")
self.max_vel = rospy.get_param("/max_vel")
self.min_vel = rospy.get_param("/min_vel")
self.max_acc = rospy.get_param("/max_acc")
self.min_acc = rospy.get_param("/min_acc")
self.max_jerk = rospy.get_param("/max_jerk")
self.min_jerk = rospy.get_param("/min_jerk")
self.max_snap = rospy.get_param("/max_snap")
self.min_snap = rospy.get_param("/min_snap")
self.done_reward = rospy.get_param("/done_reward")
self.alive_reward = rospy.get_param("/alive_reward")
self.sub_clock = rospy.Subscriber('/rosout', Log, callback_log)
self.num_agents = 4
self.num_action_space = 12
self.rate = rospy.Rate(50.0)
high = np.array([1. for _ in range(self.num_action_space)])
low = np.array([0. for _ in range(self.num_action_space)])
self.action_space = spaces.Box(low=low, high=high)
# stablishes connection with simulator
self.gazebo = GazeboConnection()
# self.controllers_object = ControllersConnection(namespace="monoped")
self.multi_uav_state_object = MultiUavState(
max_vel=self.max_vel,
min_vel=self.min_vel,
max_acc=self.max_acc,
min_acc=self.min_acc,
max_jerk=self.max_jerk,
min_jerk=self.min_jerk,
max_snap=self.max_snap,
min_snap=self.min_snap,
abs_max_roll=self.max_incl,
abs_max_pitch=self.max_incl,
done_reward=self.done_reward,
alive_reward=self.alive_reward)
self.multi_uav_state_object.set_desired_world_point(
self.desired_pose.position.x, self.desired_pose.position.y,
self.desired_pose.position.z, self.desired_pose.orientation.x,
self.desired_pose.orientation.y, self.desired_pose.orientation.z)
self.mellingers = [
Mellinger_Agent(mav_name='hummingbird',
index=0,
num_agents=4,
c=0.4,
x=0.95,
y=0.0,
z=0.35,
dim=3),
Mellinger_Agent('hummingbird',
index=1,
num_agents=4,
c=0.1,
x=0.0,
y=0.95,
z=0.35,
dim=3),
Mellinger_Agent('hummingbird',
index=2,
num_agents=4,
c=0.15,
x=-0.95,
y=0.0,
z=0.35,
dim=3),
Mellinger_Agent('hummingbird',
index=3,
num_agents=4,
c=0.35,
x=0.0,
y=-0.95,
z=0.35,
dim=3)
]
self.goal_position = np.zeros((3, 1))
# print "clock_flag: ", clock_flag
# timer_0 = rospy.Time.now()
# while not clock_flag:
# print "No clock message received!"
# try:
# rospy.wait_for_message("/clock", Clock, timeout=5.0)
# timer_1 = rospy.Time.now()
# time_spend_waiting = (timer_1 - timer_0).to_sec()
# print ("time_spend_waiting: ", time_spend_waiting)
# except:
# print "core dumped... kill"
# f_done = open(done_file,'w')
# f_done.write('1')
# f_done.close()
# break
# if time_spend_waiting > 5.0:
# print "core dumped... kill"
# f_done = open(done_file,'w')
# f_done.write('1')
# f_done.close()
"""
For this version, we consider 6 actions
1-2) Increment/Decrement haa_joint
3-4) Increment/Decrement hfe_joint
5-6) Increment/Decrement kfe_joint
"""
#self.action_space = spaces.Discrete(6)
#self.reward_range = (-np.inf, np.inf)
#self._seed()
print("end of init...")
def __init__(self):
# fixed frame is nearly on the origin of the ground 0,0,0.6 at laelaps body center of mass
self.last_base_position = [0, 0, 0]
self.distance_weight = 1
self.drift_weight = 2
self.time_step = 0.001 # default Gazebo simulation time step
self.episode_number = 0
self.frames = 0
self.torques_step = []
self.euler_angles = []
self.euler_rates = []
self.base_zaxis = []
self.base_x_y = []
self.sim_t = []
self.saving_option = False
# Rospy get parameters from config file:
self.laelaps_model_number = rospy.get_param("/laelaps_model_number")
self.ramp_model_number = rospy.get_param("/ramp_model_number")
self.ramp_available = rospy.get_param("/ramp_available")
self.env_goal = rospy.get_param("/env_goal")
self.gazebo = GazeboConnection()
self.controllers_object = ControllersConnection(
namespace="laelaps_robot", controllers_list=controllers_list)
# _______________SUBSCRIBERS__________________________________________________
# give base position and quaternions
rospy.Subscriber(gazebo_model_states_topic, ModelStates,
self.models_callback)
# give motor angle, velocity, torque
rospy.Subscriber(joint_state_topic, JointState,
self.joints_state_callback)
rospy.Subscriber("/clock", Clock, self.clock_callback)
rospy.Subscriber(rf_toe_nan_topic, Bool, self.rf_toe_nan_callback)
rospy.Subscriber(rh_toe_nan_topic, Bool, self.rh_toe_nan_callback)
rospy.Subscriber(lf_toe_nan_topic, Bool, self.lf_toe_nan_callback)
rospy.Subscriber(lh_toe_nan_topic, Bool, self.lh_toe_nan_callback)
# _______________MOTOR PUBLISHERS__________________________________________________
self.motor_pub = list()
for joint in motor_joints:
joint_controller = "/laelaps_robot/" + str(joint) + "/command"
x = rospy.Publisher(joint_controller, Float64, queue_size=1)
self.motor_pub.append(x)
#
# _______________Toe PUBLISHERS__________________________________________________
# toe4_pos_publisher node : RH_foot: toe1 , RF_foot: toe2, LF_foot: toe3, LH_foot: toe4
self.toe_pub = list()
for idx in range(len(laelaps_feet)):
toe_commands = "/laelaps_robot/toe" + str(idx + 1) + "/command"
x = rospy.Publisher(toe_commands, Toe, queue_size=1)
self.toe_pub.append(x)
# ______________Defining observation and action space____________________________
observation_high = (self.GetObservationUpperBound() + observation_eps)
observation_low = (self.GetObservationLowerBound() - observation_eps)
# Four legs toe x,y are estimated by RL
low = [x_low] * 4
low.extend([y_low] * 4)
high = [x_high] * 4
high.extend([y_high] * 4)
self.action_space = spaces.Box(low=np.array(low),
high=np.array(high),
dtype=np.float32)
# the epsilon to avoid 0 values entering the neural network in the algorithm
observation_high = (self.GetObservationUpperBound() + observation_eps)
observation_low = (self.GetObservationLowerBound() - observation_eps)
self.observation_space = spaces.Box(observation_low,
observation_high,
dtype=np.float32)
# ______________Reset and seed the environment____________________________
self.seed() # seed the environment in the initial function
self.init_reset() # reset the environment in the initial function
def __init__(self):
# initialise state
# ground truth position
self.x = 0
self.y = 0
self.z = 0
# ground truth velocity
self.x_dot = 0
self.y_dot = 0
self.z_dot = 0
# ground truth quaternion
self.imu_x = 0 # q0
self.imu_y = 0 # q1
self.imu_z = 0 # q2
self.imu_w = 0 # q3
# nav drone angle
self.rot_x = 0 # roll
self.rot_y = 0 # pitch
self.rot_z = 0 # yaw
# Optitrack Information
self.x_real = 0
self.y_real = 0
self.z_real = 0
self.dist = 0
# Subscribe to the /ardrone/navdata topic, of message type navdata, and call self.ReceiveNavdata when a message is received
self.subNavdata = rospy.Subscriber('/ardrone/navdata', Navdata,
self.receive_navdata)
self.subOdom = rospy.Subscriber('/ground_truth/state', Odometry,
self.odom_callback)
# rospy.Subscriber('/vrpn_client_node/Rigid_grant/pose', PoseStamped, self.optictrack_callback)
# Allow the controller to publish to the /ardrone/takeoff, land and reset topics
self.pubLand = rospy.Publisher('/ardrone/land', Empty, queue_size=0)
self.pubTakeoff = rospy.Publisher('/ardrone/takeoff',
Empty,
queue_size=0)
self.pubReset = rospy.Publisher('/ardrone/reset', Empty, queue_size=1)
# Allow the controller to publish to the /cmd_vel topic and thus control the drone
self.pubCommand = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# Put location into odometry
self.location = Odometry()
self.status = Navdata()
self.loc_real = PoseStamped()
# Gets parameters from param server
self.speed_value = rospy.get_param("/speed_value")
self.run_step = rospy.get_param("/run_step")
self.target_x = rospy.get_param("/desired_pose/x")
self.target_y = rospy.get_param("/desired_pose/y")
self.target_z = rospy.get_param("/desired_pose/z")
self.desired_pose = np.array(
[self.target_x, self.target_y, self.target_z])
# self.desired_pose = [0, 0, 1.5]
self.max_incl = rospy.get_param("/max_incl")
self.max_altitude = rospy.get_param("/max_altitude")
self.min_altitude = rospy.get_param("/min_altitude")
self.x_max = rospy.get_param("/max_pose/max_x")
self.y_max = rospy.get_param("/max_pose/max_y")
self.on_place = 0
self.count_on_place = 0
# initialize action space
# Forward,Left,Right,Up,Down
# self.action_space = spaces.Discrete(8)
self.action_space = spaces.Discrete(7)
self.up_bound = np.array([np.inf, np.inf, np.inf, np.inf, 1])
self.low_bound = np.array([-np.inf, -np.inf, -np.inf, -np.inf, 0])
self.observation_space = spaces.Box(
self.low_bound,
self.up_bound) # position[x,y,z], linear velocity[x,y,z]
#self.observation_space = spaces.Box(-np.inf, np.inf, (9,))
# Gazebo Connection
self.gazebo = GazeboConnection()
self._seed()
# Land the drone if we are shutting down
rospy.on_shutdown(self.send_land)
stdout=subprocess.PIPE,
shell=True,
preexec_fn=os.setsid)
time.sleep(5)
# Launch Crazyflie controllers
cf_gazebo_path = '/home/brian/catkin_ws/src/sim_cf/crazyflie_gazebo/scripts'
launch_controller_cmd = './run_cfs.sh ' + str(num_cpu)
cf_process = subprocess.Popen(launch_controller_cmd,
stdout=subprocess.PIPE,
cwd=cf_gazebo_path,
shell=True,
preexec_fn=os.setsid)
time.sleep(5)
gazebo = GazeboConnection()
gazebo.unpauseSim()
# Create the vectorized environment
env = DummyVecEnv([
make_env(env_id, i, num_cpu - 1, log_dir, gazebo,
os.getpgid(gazebo_process.pid), os.getpgid(cf_process.pid))
for i in range(num_cpu)
])
env = VecNormalize(env)
# Save best model every n steps and monitors performance
# save_best_callback = SaveOnBestTrainingRewardCallback(check_freq=250, log_dir=log_dir)
# Save model every n steps
checkpoint_callback = CheckpointCallback(save_freq=1000,
save_path='./' + log_dir,
def velocity_test():
"""
Test of the time it takes to reach a certain speed
"""
speed_value = 10.0
wait_time = 0.1
rospy.init_node('cartpole_speed_test_node',
anonymous=True,
log_level=rospy.WARN)
# Controllers Info
controllers_list = [
'joint_state_controller',
'pole_joint_velocity_controller',
'foot_joint_velocity_controller',
]
robot_name_space = "cartpole_v0"
controllers_object = ControllersConnection(
namespace=robot_name_space, controllers_list=controllers_list)
debug_object = MoveCartClass()
start_init_physics_parameters = True
reset_world_or_sim = "SIMULATION"
gazebo = GazeboConnection(start_init_physics_parameters,
reset_world_or_sim)
rospy.loginfo("RESETING SIMULATION")
gazebo.pauseSim()
gazebo.resetSim()
gazebo.unpauseSim()
rospy.loginfo("CLOCK AFTER RESET")
debug_object.get_clock_time()
rospy.loginfo("RESETING CONTROLLERS SO THAT IT DOESNT WAIT FOR THE CLOCK")
controllers_object.reset_controllers()
rospy.loginfo("AFTER RESET CHECKING SENSOR DATA")
debug_object.check_all_systems_ready()
rospy.loginfo("CLOCK AFTER SENSORS WORKING AGAIN")
debug_object.get_clock_time()
rospy.loginfo("START CHECKING SENSOR DATA")
debug_object.check_all_systems_ready()
rospy.loginfo("SET init pose...")
debug_object.set_init_pose()
rospy.loginfo("WAIT FOR GOING TO INIT POSE")
time.sleep(wait_time)
raw_input("Start Movement...PRESS KEY")
i = 0
wait_times_m = []
while not rospy.is_shutdown():
vel_x = [speed_value]
rospy.loginfo("Moving RIGHT...")
debug_object.move_joints([speed_value])
delta_time = debug_object.wait_until_base_is_in_vel([speed_value])
wait_times_m = numpy.append(wait_times_m, [delta_time])
rospy.loginfo("Moving STOP...")
debug_object.move_joints([0.0])
delta_time = debug_object.wait_until_base_is_in_vel(0.0)
wait_times_m = numpy.append(wait_times_m, [delta_time])
raw_input("Start Movement...PRESS KEY")
rospy.loginfo("Moving LEFT...")
debug_object.move_joints([-1 * speed_value])
delta_time = debug_object.wait_until_base_is_in_vel([-1 * speed_value])
wait_times_m = numpy.append(wait_times_m, [delta_time])
rospy.loginfo("Moving STOP...")
debug_object.move_joints([0.0])
delta_time = debug_object.wait_until_base_is_in_vel(0.0)
wait_times_m = numpy.append(wait_times_m, [delta_time])
raw_input("Start Movement...PRESS KEY")
i += 1
if i > 10:
average_time = numpy.average(wait_times_m)
rospy.logwarn("[average_time Wait Time=" + str(average_time) + "]")
break
def __init__(self):
self.number_actions = rospy.get_param('/moving_cube/n_actions')
self.field_division = rospy.get_param("/moving_cube/field_division")
self.seed()
self.viewer = None
self.action_space = spaces.Box(0.0,
0.5, (self.ACTION_SIZE, ),
dtype=np.float32)
self.observation_space = spaces.Box(-np.inf,
+np.inf,
shape=(self.STATE_SIZE, ),
dtype=np.float32)
# get configuration parameters
self.init_roll_vel = rospy.get_param('/moving_cube/init_roll_vel')
self.get_link_state = rospy.ServiceProxy("/gazebo/get_link_state",
GetLinkState)
# Actions
self.roll_speed_fixed_value = rospy.get_param(
'/moving_cube/roll_speed_fixed_value')
self.roll_speed_increment_value = rospy.get_param(
'/moving_cube/roll_speed_increment_value')
self.start_point = Point()
self.start_point.x = rospy.get_param("/moving_cube/init_cube_pose/x")
self.start_point.y = rospy.get_param("/moving_cube/init_cube_pose/y")
self.start_point.z = rospy.get_param("/moving_cube/init_cube_pose/z")
self.hyd = 0
self.bucket_vel = 0
self.depth = 0
self.m = (2.1213) / (1.9213 + 0.2) # the slope of the pile
self.density = 1922 # density of material in kg/m^3
self.z_collision = 0
self.last_volume = 0
self.volume_sum = 0
self.last_z_pile = 0
self.last_x_step = 0
self.last_z_collision = 0
self.tip_position = Point()
self.last_reward = 0
self.pitch = 0
self.counter = 0
self.c = 0
self.flag = 0
self.max_volume = 850 # max bucket operation load for tool
# Done
self.max_pitch_angle = rospy.get_param('/moving_cube/max_pitch_angle')
# Rewards
self.move_distance_reward_weight = rospy.get_param(
"/moving_cube/move_distance_reward_weight")
self.y_linear_speed_reward_weight = rospy.get_param(
"/moving_cube/y_linear_speed_reward_weight")
self.y_axis_angle_reward_weight = rospy.get_param(
"/moving_cube/y_axis_angle_reward_weight")
self.end_episode_points = rospy.get_param(
"/moving_cube/end_episode_points")
# stablishes connection with simulator
self.gazebo = GazeboConnection()
self.gazebo.unpauseSim()
self.check_all_sensors_ready()
self.pub_bucket_tip = rospy.Publisher('/bobcat/tip_position',
Point,
queue_size=10)
rospy.Subscriber('/bobcat/arm/hydraulics', Float64, self.get_hyd)
rospy.Subscriber('/WPD/Speed', TwistStamped, self.get_vel)
rospy.Subscriber('/bobcat/arm/loader', Float64, self.get_bucket_vel)
rospy.Subscriber("/Bobby/joint_states", JointState,
self.joints_callback)
rospy.Subscriber("/Bobby/odom", Odometry, self.odom_callback)
rospy.Subscriber('/robot_bumper', ContactsState, self.get_depth)
rospy.Subscriber('/Bobby/imu', Imu, self.get_angular_vel)
rospy.Subscriber("/gazebo/link_states", LinkStates,
self.cb_link_states)
self.pub = rospy.Publisher('/WPD/Speed', TwistStamped, queue_size=10)
self.pub2 = rospy.Publisher('/bobcat/arm/hydraulics',
Float64,
queue_size=10)
self.pub3 = rospy.Publisher('/bobcat/arm/loader',
Float64,
queue_size=10)
self.pub4 = rospy.Publisher('/volume', Float32, queue_size=10)
self.pub5 = rospy.Publisher('/penetration_z', Float32, queue_size=10)
self.gazebo.pauseSim()
self.command = TwistStamped()
self.node_process = Popen(
shlex.split('rosrun robil_lihi C_response_loader.py'))
self.node_process.terminate()
def position_reset_test():
"""
Test of position accuracy and reset system.
"""
position = 0.0
position = float(sys.argv[1])
rospy.init_node('debug_test_node', anonymous=True, log_level=rospy.WARN)
rospy.logwarn("[position=" + str(position) + "]")
wait_time = 0.1
controllers_object = ControllersConnection(namespace="cartpole_v0")
debug_object = MoveCartClass()
gazebo = GazeboConnection()
rospy.loginfo("RESETING SIMULATION")
gazebo.pauseSim()
gazebo.resetSim()
gazebo.unpauseSim()
rospy.loginfo("CLOCK AFTER RESET")
debug_object.get_clock_time()
rospy.loginfo("RESETING CONTROLLERS SO THAT IT DOESNT WAIT FOR THE CLOCK")
controllers_object.reset_controllers()
rospy.loginfo("AFTER RESET CHECKING SENSOR DATA")
debug_object.check_all_systems_ready()
rospy.loginfo("CLOCK AFTER SENSORS WORKING AGAIN")
debug_object.get_clock_time()
rospy.loginfo("START CHECKING SENSOR DATA")
debug_object.check_all_systems_ready()
rospy.loginfo("SET init pose...")
debug_object.set_init_pose()
rospy.loginfo("WAIT FOR GOING TO INIT POSE")
time.sleep(wait_time)
raw_input("Start Movement...PRESS KEY")
i = 0
wait_times_m = []
while not rospy.is_shutdown():
pos_x = [position]
debug_object.move_joints(pos_x)
delta_time = debug_object.wait_until_base_is_in_pos(position)
wait_times_m = numpy.append(wait_times_m, [delta_time])
debug_object.move_joints([0.0])
delta_time = debug_object.wait_until_base_is_in_pos(0.0)
wait_times_m = numpy.append(wait_times_m, [delta_time])
i += 1
if i > 10:
average_time = numpy.average(wait_times_m)
rospy.logwarn("[average_time Wait Time=" + str(average_time) + "]")
break
rospy.loginfo("END CHECKING SENSOR DATA")
debug_object.check_all_systems_ready()
rospy.loginfo("CLOCK BEFORE RESET")
debug_object.get_clock_time()
# Reset Sim
raw_input("END Start Movement...PRESS KEY")
rospy.loginfo("SETTING INITIAL POSE TO AVOID")
debug_object.set_init_pose()
time.sleep(wait_time * 2.0)
rospy.loginfo("AFTER INITPOSE CHECKING SENSOR DATA")
debug_object.check_all_systems_ready()
rospy.loginfo(
"We deactivate gravity to check any reasidual effect of reseting the simulation"
)
gazebo.change_gravity(0.0, 0.0, 0.0)
rospy.loginfo("RESETING SIMULATION")
gazebo.pauseSim()
gazebo.resetSim()
gazebo.unpauseSim()
rospy.loginfo("CLOCK AFTER RESET")
debug_object.get_clock_time()
rospy.loginfo("RESETING CONTROLLERS SO THAT IT DOESNT WAIT FOR THE CLOCK")
controllers_object.reset_controllers()
rospy.loginfo("AFTER RESET CHECKING SENSOR DATA")
debug_object.check_all_systems_ready()
rospy.loginfo("CLOCK AFTER SENSORS WORKING AGAIN")
debug_object.get_clock_time()
rospy.loginfo("We reactivating gravity...")
gazebo.change_gravity(0.0, 0.0, -9.81)
rospy.loginfo("END")
def __init__(self):
rospy.logdebug("Starting URSimDoorOpening Class object...")
# Init GAZEBO Objects
self.set_obj_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
self.get_world_state = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
# Subscribe joint state and target pose
rospy.Subscriber("/ft_sensor_topic", WrenchStamped, self.wrench_stamped_callback)
rospy.Subscriber("/joint_states", JointState, self.joints_state_callback)
rospy.Subscriber("/gazebo/link_states", LinkStates, self.link_state_callback)
# Gets training parameters from param server
self.desired_pose = Pose()
self.running_step = rospy.get_param("/running_step")
self.max_height = rospy.get_param("/max_height")
self.min_height = rospy.get_param("/min_height")
self.observations = rospy.get_param("/observations")
# Joint limitation
shp_max = rospy.get_param("/joint_limits_array/shp_max")
shp_min = rospy.get_param("/joint_limits_array/shp_min")
shl_max = rospy.get_param("/joint_limits_array/shl_max")
shl_min = rospy.get_param("/joint_limits_array/shl_min")
elb_max = rospy.get_param("/joint_limits_array/elb_max")
elb_min = rospy.get_param("/joint_limits_array/elb_min")
wr1_max = rospy.get_param("/joint_limits_array/wr1_max")
wr1_min = rospy.get_param("/joint_limits_array/wr1_min")
wr2_max = rospy.get_param("/joint_limits_array/wr2_max")
wr2_min = rospy.get_param("/joint_limits_array/wr2_min")
wr3_max = rospy.get_param("/joint_limits_array/wr3_max")
wr3_min = rospy.get_param("/joint_limits_array/wr3_min")
self.joint_limits = {"shp_max": shp_max,
"shp_min": shp_min,
"shl_max": shl_max,
"shl_min": shl_min,
"elb_max": elb_max,
"elb_min": elb_min,
"wr1_max": wr1_max,
"wr1_min": wr1_min,
"wr2_max": wr2_max,
"wr2_min": wr2_min,
"wr3_max": wr3_max,
"wr3_min": wr3_min
}
shp_init_value0 = rospy.get_param("/init_joint_pose0/shp")
shl_init_value0 = rospy.get_param("/init_joint_pose0/shl")
elb_init_value0 = rospy.get_param("/init_joint_pose0/elb")
wr1_init_value0 = rospy.get_param("/init_joint_pose0/wr1")
wr2_init_value0 = rospy.get_param("/init_joint_pose0/wr2")
wr3_init_value0 = rospy.get_param("/init_joint_pose0/wr3")
self.init_joint_pose0 = [shp_init_value0, shl_init_value0, elb_init_value0, wr1_init_value0, wr2_init_value0, wr3_init_value0]
shp_init_value1 = rospy.get_param("/init_joint_pose1/shp")
shl_init_value1 = rospy.get_param("/init_joint_pose1/shl")
elb_init_value1 = rospy.get_param("/init_joint_pose1/elb")
wr1_init_value1 = rospy.get_param("/init_joint_pose1/wr1")
wr2_init_value1 = rospy.get_param("/init_joint_pose1/wr2")
wr3_init_value1 = rospy.get_param("/init_joint_pose1/wr3")
self.init_joint_pose1 = [shp_init_value1, shl_init_value1, elb_init_value1, wr1_init_value1, wr2_init_value1, wr3_init_value1]
# print("[init_joint_pose1]: ", [shp_init_value1, shl_init_value1, elb_init_value1, wr1_init_value1, wr2_init_value1, wr3_init_value1])
shp_init_value2 = rospy.get_param("/init_joint_pose2/shp")
shl_init_value2 = rospy.get_param("/init_joint_pose2/shl")
elb_init_value2 = rospy.get_param("/init_joint_pose2/elb")
wr1_init_value2 = rospy.get_param("/init_joint_pose2/wr1")
wr2_init_value2 = rospy.get_param("/init_joint_pose2/wr2")
wr3_init_value2 = rospy.get_param("/init_joint_pose2/wr3")
self.init_joint_pose2 = [shp_init_value2, shl_init_value2, elb_init_value2, wr1_init_value2, wr2_init_value2, wr3_init_value2]
# print("[init_joint_pose2]: ", [shp_init_value2, shl_init_value2, elb_init_value2, wr1_init_value2, wr2_init_value2, wr3_init_value2])
shp_after_rotate = rospy.get_param("/eelink_pose_after_rotate/shp")
shl_after_rotate = rospy.get_param("/eelink_pose_after_rotate/shl")
elb_after_rotate = rospy.get_param("/eelink_pose_after_rotate/elb")
wr1_after_rotate = rospy.get_param("/eelink_pose_after_rotate/wr1")
wr2_after_rotate = rospy.get_param("/eelink_pose_after_rotate/wr2")
wr3_after_rotate = rospy.get_param("/eelink_pose_after_rotate/wr3")
self.after_rotate = [shp_after_rotate, shl_after_rotate, elb_after_rotate, wr1_after_rotate, wr2_after_rotate, wr3_after_rotate]
shp_after_pull = rospy.get_param("/eelink_pose_after_pull/shp")
shl_after_pull = rospy.get_param("/eelink_pose_after_pull/shl")
elb_after_pull = rospy.get_param("/eelink_pose_after_pull/elb")
wr1_after_pull = rospy.get_param("/eelink_pose_after_pull/wr1")
wr2_after_pull = rospy.get_param("/eelink_pose_after_pull/wr2")
wr3_after_pull = rospy.get_param("/eelink_pose_after_pull/wr3")
self.after_pull = [shp_after_pull, shl_after_pull, elb_after_pull, wr1_after_pull, wr2_after_pull, wr3_after_pull]
r_drv_value1 = rospy.get_param("/init_grp_pose1/r_drive")
l_drv_value1 = rospy.get_param("/init_grp_pose1/l_drive")
r_flw_value1 = rospy.get_param("/init_grp_pose1/r_follower")
l_flw_value1 = rospy.get_param("/init_grp_pose1/l_follower")
r_spr_value1 = rospy.get_param("/init_grp_pose1/r_spring")
l_spr_value1 = rospy.get_param("/init_grp_pose1/l_spring")
r_drv_value2 = rospy.get_param("/init_grp_pose2/r_drive")
l_drv_value2 = rospy.get_param("/init_grp_pose2/l_drive")
r_flw_value2 = rospy.get_param("/init_grp_pose2/r_follower")
l_flw_value2 = rospy.get_param("/init_grp_pose2/l_follower")
r_spr_value2 = rospy.get_param("/init_grp_pose2/r_spring")
l_spr_value2 = rospy.get_param("/init_grp_pose2/l_spring")
self.init_grp_pose1 = [r_drv_value1, l_drv_value1, r_flw_value1, l_flw_value1, r_spr_value1, l_spr_value1]
self.init_grp_pose2 = [r_drv_value2, l_drv_value2, r_flw_value2, l_flw_value2, r_spr_value2, l_spr_value2]
# Fill in the Done Episode Criteria list
self.episode_done_criteria = rospy.get_param("/episode_done_criteria")
# stablishes connection with simulator
self._gz_conn = GazeboConnection()
self._ctrl_conn = ControllersConnection(namespace="")
# Controller type for ros_control
self._ctrl_type = rospy.get_param("/control_type")
self.pre_ctrl_type = self._ctrl_type
# Get the force and troque limit
self.force_limit = rospy.get_param("/force_limit")
self.torque_limit = rospy.get_param("/torque_limit")
# We init the observations
self.base_orientation = Quaternion()
self.imu_link = Quaternion()
self.door = Quaternion()
self.quat = Quaternion()
self.imu_link_rpy = Vector3()
self.door_rpy = Vector3()
self.link_state = LinkStates()
self.wrench_stamped = WrenchStamped()
self.joints_state = JointState()
self.end_effector = Point()
self.previous_action =[]
self.counter = 0
self.max_rewards = 1
# Arm/Control parameters
self._ik_params = setups['UR5_6dof']['ik_params']
# ROS msg type
self._joint_pubisher = JointPub()
self._joint_traj_pubisher = JointTrajPub()
# Gym interface and action
self.action_space = spaces.Discrete(6)
self.observation_space = 21 #np.arange(self.get_observations().shape[0])
## self.observation_space = 15 #np.arange(self.get_observations().shape[0])
self.reward_range = (-np.inf, np.inf)
self._seed()
# Change the controller type
set_joint_pos_server = rospy.Service('/set_position_controller', SetBool, self._set_pos_ctrl)
set_joint_traj_pos_server = rospy.Service('/set_trajectory_position_controller', SetBool, self._set_traj_pos_ctrl)
set_joint_vel_server = rospy.Service('/set_velocity_controller', SetBool, self._set_vel_ctrl)
set_joint_traj_vel_server = rospy.Service('/set_trajectory_velocity_controller', SetBool, self._set_traj_vel_ctrl)
# set_gripper_server = rospy.Service('/set_gripper_controller', SetBool, self._set_grp_ctrl)
self.pos_traj_controller = ['joint_state_controller',
'gripper_controller',
'pos_traj_controller']
self.pos_controller = ["joint_state_controller",
"gripper_controller",
"ur_shoulder_pan_pos_controller",
"ur_shoulder_lift_pos_controller",
"ur_elbow_pos_controller",
"ur_wrist_1_pos_controller",
"ur_wrist_2_pos_controller",
"ur_wrist_3_pos_controller"]
self.vel_traj_controller = ['joint_state_controller',
'gripper_controller',
'vel_traj_controller']
self.vel_controller = ["joint_state_controller",
"gripper_controller",
"ur_shoulder_pan_vel_controller",
"ur_shoulder_lift_vel_controller",
"ur_elbow_vel_controller",
"ur_wrist_1_vel_controller",
"ur_wrist_2_vel_controller",
"ur_wrist_3_vel_controller"]
# Helpful False
self.stop_flag = False
stop_trainning_server = rospy.Service('/stop_training', SetBool, self._stop_trainnig)
start_trainning_server = rospy.Service('/start_training', SetBool, self._start_trainnig)