def __init__(self):
rospy.init_node('bumper_test')
self.bumper_topic_name = "~bumper_topic_name"
self.bumper_topic = "/test_bumper"
self.bumper_state = ContactsState()
self.sample_count = 0
rospy.Subscriber(self.bumper_topic, ContactsState,
self.bumperStateInput)
def __init__(self, reward_type="hand_craft", set_additional_goal="None"):
# Build a ros node
rospy.init_node('DubinsCarEnv', anonymous=True)
# Define necessary ros services
self.vel_pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=5)
self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
self.reset_world = rospy.ServiceProxy('/gazebo/reset_world', Empty)
self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# Define ros subscriber for receiving sensor readings. Note: this ways is more stable than using "wait_for_message"
rospy.Subscriber("/scan", LaserScan, self._laser_scan_callback)
rospy.Subscriber("/gazebo_ros_bumper", ContactsState, self._contact_callback)
self.laser_data = LaserScan()
self.contact_data = ContactsState()
# Task-specific settings
self.num_lasers = 8
self.state_dim = 5 # x,y,theta,v,w
self.action_dim = 2 # vel_acc, ang_acc
high_state = np.array([5.0, 5.0, np.pi, 5.0, np.pi*5])
low_state = np.array([-5.0, -5.0, -np.pi, -5.0, -np.pi*5])
high_obsrv = np.concatenate((high_state, np.array([5 * 2] * self.num_lasers)), axis=0)
low_obsrv = np.concatenate((low_state, np.array([0] * self.num_lasers)), axis=0)
# This is consistent with the step() function clipping range.
# Since you cannot expect NN output is in line with your real physics property. So there always needs transformation
high_action = np.array([2.0, 2.0])
low_action = np.array([-2.0, -2.0])
self.state_space = spaces.Box(low=low_state, high=high_state)
self.observation_space = spaces.Box(low=low_obsrv, high=high_obsrv)
self.action_space = spaces.Box(low=low_action, high=high_action)
self.goal_state = GOAL_STATE
self.start_state = START_STATE
# goal tolerance definition
self.goal_pos_tolerance = 1.0
self.goal_theta_tolerance = 0.75 # around 45 degrees
self.control_reward_coff = 0.01
self.collision_reward = -2 * 200 * self.control_reward_coff*(10**2)
self.goal_reward = 1000
self.pre_obsrv = None
self.pre_vel = None
self.pre_angvel = None
self.reward_type = reward_type
self.set_additional_goal = set_additional_goal
self.step_counter = 0
self._max_episode_steps = 300
logger.log("DubinsCarEnv-v1 is successfully initialized!!")
def force_listener(self, msg):
#print(msg)
force = self.threeDVecMsg2Numpy(msg.wrench.force)
#print(force)
tool_weight_msg = Vector3Stamped()
tool_weight_msg.header = deepcopy(msg.header)
tool_weight_msg.header.frame_id = "world"
tool_weight_msg.vector.z = 1.0 # about 100 g...
start = rospy.Time.now()
while not self.tf_listener.canTransform("world", msg.header.frame_id,
msg.header.stamp):
rospy.sleep(0.001) # wait for sync in tf
if rospy.Time.now() > start + rospy.Duration.from_sec(0.5):
print("No Transform")
return
tool_weight_msg = self.tf_listener.transformVector3(
msg.header.frame_id, tool_weight_msg)
#print(tool_weight_msg)
tool_weight_force = self.threeDVecMsg2Numpy(tool_weight_msg.vector)
net_force = force - tool_weight_force
if np.linalg.norm(net_force) < ABS_FORCE_THRES:
return
j_state = rospy.wait_for_message("/joint_states", JointState)
if np.linalg.norm(j_state.velocity) > 0.1:
print("Too fast")
return
# TODO: Filter to high accelertations of eef
normal = net_force / np.linalg.norm(net_force)
c_point = normal * (0.015)
c_point_msg = PointStamped()
c_point_msg.header = deepcopy(msg.header)
c_point_msg.point = Point(*c_point)
c_pt_global = self.tf_listener.transformPoint("world", c_point_msg)
print(c_pt_global)
ret_msg = ContactsState()
ret_msg.header = deepcopy(c_pt_global.header)
contact_state = ContactState()
contact_state.contact_positions.append(c_pt_global.point)
ret_msg.states.append(contact_state)
self.coll_pub.publish(ret_msg)
def touch_and_refine_table(robot, scene, move_group):
urdf_robot = URDF.from_parameter_server()
kdl_kin = KDLKinematics(urdf_robot, urdf_robot.links[1].name,
urdf_robot.links[8].name)
move_group.set_max_velocity_scaling_factor(
0.1) # Allow 10 % of the set maximum joint velocities
move_group.set_max_acceleration_scaling_factor(0.05)
# Receive table message
go_home()
print("Waiting for table message ...")
the_table = rospy.wait_for_message("/octomap_new/table", table)
touchPoses = []
for i in range(10):
pose_goal = go_to_random_point_over_table(move_group, the_table)
touchSensorMsg = touch_table_straight_on(move_group, robot, kdl_kin,
pose_goal)
print("Touched the surface")
# Check if usable collision
if '/panda/bumper/panda_probe_ball' in touchSensorMsg.collidingLinks:
rospy.sleep(2.)
ballSensorMsg = ContactsState()
while ballSensorMsg.states == []:
ballSensorMsg = rospy.wait_for_message(
"/panda/bumper/panda_probe_ball", ContactsState)
#print(ballSensorMsg)
touchPoses.append(ballSensorMsg)
else:
print("Collided with wrong part; ignored")
i -= 1
print("Recording done, retracting ...")
retract_from_table_straight(move_group, robot, kdl_kin)
# note eef position when collided, e.g. by listening to /panda/panda/colliding; in real probably ask libfranka
fit_table(touchPoses)
def go_straight_till_coll(self, x_step=0, y_step=0, z_step=0):
""" Moves along step direction till a collision is sensed; if probe_ball collided returns collisionstate else None """
pos_to_keep = self.get_current_position()
ori_to_keep = self.get_current_rotation()
IK_retries = 10
while IK_retries > 0:
pos_to_keep[0] += x_step
pos_to_keep[1] += y_step
pos_to_keep[2] += z_step
IK_retries += self.p_step(pos_to_keep, ori_to_keep)
rospy.sleep(0.8)
touchSensorMsg = rospy.wait_for_message("/panda/bumper/colliding",
CollisionState)
if touchSensorMsg.colliding:
print("Detected Collision")
if '/panda/bumper/panda_probe_ball' in touchSensorMsg.collidingLinks: # Collided with sensing part -> wait for details
ballSensorMsg = ContactsState()
start = rospy.Time.now()
while ballSensorMsg.states == []:
try:
ballSensorMsg = rospy.wait_for_message(
"/panda/bumper/panda_probe_ball",
ContactsState,
timeout=0.5)
break
except:
print("Time out on collision")
if rospy.Time.now() > start + rospy.Duration.from_sec(
3): # Don't wait for ever
print("False alarm")
break
print(ballSensorMsg)
if len(ballSensorMsg.states) > 0:
print("Touched with state: \n{}".format(ballSensorMsg))
return ballSensorMsg
else:
continue
else: # Collided with wrong part -> ignore this collision in later evaluation
return None
def __init__(self, *args):
super(BumperTest, self).__init__(*args)
self.success = False
self.sample_count = 0
self.min_samples_topic = "~min_samples"
self.min_samples = 1000
# in seconds
self.test_duration_topic = "~test_duration"
self.test_duration = 10.0
# test start time in seconds
self.test_start_time_topic = "~test_start_time"
self.test_start_time = 0.0
self.bumper_topic_name = "~bumper_topic_name"
self.bumper_topic = "/test_bumper"
self.bumper_state = ContactsState()
self.fz_sum = 0
self.fz_avg = 0
def __init__(self, joint_increment_value=0.02, running_step=0.001):
"""
initializing all the relevant variables and connections
"""
# Assign Parameters
self._joint_increment_value = joint_increment_value
self.running_step = running_step
# Assign MsgTypes
self.joints_state = JointState()
self.contact_1_state = ContactsState()
self.contact_2_state = ContactsState()
self.collision = Bool()
self.camera_rgb_state = Image()
self.camera_depth_state = Image()
self.contact_1_force = Vector3()
self.contact_2_force = Vector3()
self.gripper_state = VacuumGripperState()
self._list_of_observations = ["distance_gripper_to_object",
"elbow_joint_state",
"shoulder_lift_joint_state",
"shoulder_pan_joint_state",
"wrist_1_joint_state",
"wrist_2_joint_state",
"wrist_3_joint_state",
"contact_1_force",
"contact_2_force",
"object_pos_x",
"object_pos_y",
"object_pos_z"]
# Establishes connection with simulator
"""
1) Gazebo Connection
2) Controller Connection
3) Joint Publisher
"""
self.gazebo = GazeboConnection()
self.controllers_object = ControllersConnection()
self.pickbot_joint_publisher_object = JointPub()
# Define Subscribers as Sensor data
"""
1) /pickbot/joint_states
2) /gripper_contactsensor_1_state
3) /gripper_contactsensor_2_state
4) /gz_collisions
not used so far but available in the environment
5) /pickbot/gripper/state
6) /camera_rgb/image_raw
7) /camera_depth/depth/image_raw
"""
rospy.Subscriber("/pickbot/joint_states", JointState, self.joints_state_callback)
rospy.Subscriber("/gripper_contactsensor_1_state", ContactsState, self.contact_1_callback)
rospy.Subscriber("/gripper_contactsensor_2_state", ContactsState, self.contact_2_callback)
rospy.Subscriber("/gz_collisions", Bool, self.collision_callback)
rospy.Subscriber("/pickbot/gripper/state", VacuumGripperState, self.gripper_state_callback)
# rospy.Subscriber("/camera_rgb/image_raw", Image, self.camera_rgb_callback)
# rospy.Subscriber("/camera_depth/depth/image_raw", Image, self.camera_depth_callback)
# Define Action and state Space and Reward Range
"""
Action Space: Discrete with 13 actions
1-2) Increment/Decrement joint1_position_controller
3-4) Increment/Decrement joint2_position_controller
5-6) Increment/Decrement joint3_position_controller
7-8) Increment/Decrement joint4_position_controller
9-10) Increment/Decrement joint5_position_controller
11-12) Increment/Decrement joint6_position_controller
13) Turn on/off the vacuum gripper
State Space: Box Space with 13 values. It is a numpy array with shape (13,)
Reward Range: -infinity to infinity
"""
self.action_space = spaces.Discrete(13)
high = np.array([
1,
math.pi,
math.pi,
math.pi,
math.pi,
math.pi,
math.pi,
np.finfo(np.float32).max,
np.finfo(np.float32).max,
1,
1.4,
1.5])
low = np.array([
0,
-math.pi,
-math.pi,
-math.pi,
-math.pi,
-math.pi,
-math.pi,
0,
0,
-1,
0,
0])
self.observation_space = spaces.Box(low, high)
self.reward_range = (-np.inf, np.inf)
self._seed()
self.done_reward = 0
# set up everything to publish the Episode Number and Episode Reward on a rostopic
self.episode_num = 0
self.cumulated_episode_reward = 0
self.episode_steps = 0
self.reward_pub = rospy.Publisher('/openai/reward', RLExperimentInfo, queue_size=1)
self.reward_list = []
self.episode_list = []
self.step_list = []
self.csv_name = logger.get_dir() + '/result_log'
print("CSV NAME")
print(self.csv_name)
def __init__(self):
"""Initializes a new Robot environment.
"""
# Variables that we give through the constructor.
# Internal Vars
self.controllers_list = [
'joint_1_position_controller', 'joint_2_position_controller',
'joint_3_position_controller', 'joint_4_position_controller',
'joint_5_position_controller', 'joint_6_position_controller',
'finger_1_position_controller', 'finger_2_position_controller',
'finger_3_position_controller', 'joint_state_controller'
]
self.robot_name_space = "j2n6s300"
reset_controls_bool = True
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(j2n6s300Env,
self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=reset_controls_bool,
reset_world_or_sim="WORLD")
# We Start all the ROS related Subscribers and publishers
self.joint_states_sub = rospy.Subscriber('/j2n6s300/joint_states',
JointState,
self.joint_states_callback)
#self.link_states_sub = rospy.Subscriber('/j2n6s300/link_states', LinkStates, self.link_states_callback)
self.link_states_sub = rospy.Subscriber('/gazebo/link_states',
LinkStates,
self.link_states_callback)
self.collision_sub = rospy.Subscriber('/j2n6s300/collision',
ContactsState,
self.collision_callback)
self.pub = rospy.Publisher(
'/j2n6s300/effort_joint_trajectory_controller/command',
JointTrajectory,
queue_size=1)
self.pubf = rospy.Publisher(
'/j2n6s300/effort_finger_trajectory_controller/command',
JointTrajectory,
queue_size=1)
self.joint_1_pos_pub = rospy.Publisher(
'/j2n6s300/joint_1_position_controller/command',
Float64,
queue_size=1)
self.joint_2_pos_pub = rospy.Publisher(
'/j2n6s300/joint_2_position_controller/command',
Float64,
queue_size=1)
self.joint_3_pos_pub = rospy.Publisher(
'/j2n6s300/joint_3_position_controller/command',
Float64,
queue_size=1)
self.joint_4_pos_pub = rospy.Publisher(
'/j2n6s300/joint_4_position_controller/command',
Float64,
queue_size=1)
self.joint_5_pos_pub = rospy.Publisher(
'/j2n6s300/joint_5_position_controller/command',
Float64,
queue_size=1)
self.joint_6_pos_pub = rospy.Publisher(
'/j2n6s300/joint_6_position_controller/command',
Float64,
queue_size=1)
self.finger_1_tip_pub = rospy.Publisher(
'/j2n6s300/finger_tip_1_position_controller/command',
Float64,
queue_size=1)
self.finger_2_tip_pub = rospy.Publisher(
'/j2n6s300/finger_tip_2_position_controller/command',
Float64,
queue_size=1)
self.finger_3_tip_pub = rospy.Publisher(
'/j2n6s300/finger_tip_3_position_controller/command',
Float64,
queue_size=1)
self.collision_states = ContactsState()
def __init__(self,
joint_increment_value=0.02,
sim_time_factor=0.001,
running_step=0.001,
random_object=False,
random_position=False,
use_object_type=False,
env_object_type='free_shapes',
load_init_pos=False):
"""
initializing all the relevant variables and connections
:param joint_increment_value: increment of the joints
:param running_step: gazebo simulation time factor
:param random_object: spawn random object in the simulation
:param random_position: change object position in each reset
:param use_object_type: assign IDs to objects and used them in the observation space
:param env_object_type: object type for environment, free_shapes for boxes while others are related to use_case
'door_handle', 'combox', ...
"""
# Assign Parameters
self._joint_increment_value = joint_increment_value
self.running_step = running_step
self._random_object = random_object
self._random_position = random_position
self._use_object_type = use_object_type
self._load_init_pos = load_init_pos
# Assign MsgTypes
self.joints_state = JointState()
self.contact_1_state = ContactsState()
self.contact_2_state = ContactsState()
self.collision = Bool()
self.camera_rgb_state = Image()
self.camera_depth_state = Image()
self.contact_1_force = Vector3()
self.contact_2_force = Vector3()
self.gripper_state = VacuumGripperState()
self._list_of_observations = [
"elbow_joint_state",
"shoulder_lift_joint_state",
"shoulder_pan_joint_state",
"wrist_1_joint_state",
"wrist_2_joint_state",
"wrist_3_joint_state",
"vacuum_gripper_pos_x",
"vacuum_gripper_pos_y",
"vacuum_gripper_pos_z",
"vacuum_gripper_ori_w",
"vacuum_gripper_ori_x",
"vacuum_gripper_ori_y",
"vacuum_gripper_ori_z",
"object_pos_x",
"object_pos_y",
"object_pos_z",
"object_ori_w",
"object_ori_x",
"object_ori_y",
"object_ori_z",
]
# if self._use_object_type:
# self._list_of_observations.append("object_type")
# Establishes connection with simulator
"""
1) Gazebo Connection
2) Controller Connection
3) Joint Publisher
"""
self.gazebo = GazeboConnection(sim_time_factor=sim_time_factor)
self.controllers_object = ControllersConnection()
self.pickbot_joint_publisher_object = JointPub()
# Define Subscribers as Sensordata
"""
1) /pickbot/joint_states
2) /gripper_contactsensor_1_state
3) /gripper_contactsensor_2_state
4) /gz_collisions
5) /pickbot/gripper/state
6) /camera_rgb/image_raw
7) /camera_depth/depth/image_raw
"""
rospy.Subscriber("/pickbot/joint_states", JointState,
self.joints_state_callback)
rospy.Subscriber("/gripper_contactsensor_1_state", ContactsState,
self.contact_1_callback)
rospy.Subscriber("/gripper_contactsensor_2_state", ContactsState,
self.contact_2_callback)
rospy.Subscriber("/gz_collisions", Bool, self.collision_callback)
rospy.Subscriber("/pickbot/gripper/state", VacuumGripperState,
self.gripper_state_callback)
# rospy.Subscriber("/camera_rgb/image_raw", Image, self.camera_rgb_callback)
# rospy.Subscriber("/camera_depth/depth/image_raw", Image, self.camera_depth_callback)
# Define Action and state Space and Reward Range
"""
Action Space: Box Space with 6 values.
State Space: Box Space with 20 values. It is a numpy array with shape (20,)
Reward Range: -infinity to infinity
"""
# Directly use joint_positions as action
if self._joint_increment_value is None:
high_action = (math.pi - 0.05) * np.ones(6)
low_action = -high_action
else: # Use joint_increments as action
high_action = self._joint_increment_value * np.ones(6)
low_action = -high_action
self.action_space = spaces.Box(low_action, high_action)
self.obs_dim = 20
high = np.inf * np.ones(self.obs_dim)
low = -high
self.observation_space = spaces.Box(low, high)
# if self._use_object_type:
# high = np.append(high, 9)
# low = np.append(low, 0)
self.reward_range = (-np.inf, np.inf)
self._seed()
self.done_reward = 0
# set up everything to publish the Episode Number and Episode Reward on a rostopic
self.episode_num = 0
self.accumulated_episode_reward = 0
self.episode_steps = 0
self.reward_pub = rospy.Publisher('/openai/reward',
RLExperimentInfo,
queue_size=1)
self.reward_list = []
self.episode_list = []
self.step_list = []
self.csv_name = logger.get_dir() + '/result_log'
print("CSV NAME")
print(self.csv_name)
self.csv_success_exp = logger.get_dir(
) + '/success_exp' + datetime.datetime.now().strftime(
'%Y-%m-%d_%Hh%Mmin') + '.csv'
self.successful_attempts = 0
# variable to store last observation
self.old_obs = self.get_obs()
# object name: name of the target object
# object type: index of the object name in the object list
# object list: pool of the available objects, have at least one entry
self.object_name = ''
self.object_type_str = ''
self.object_type = 0
self.object_list = U.get_target_object(env_object_type)
print("object list {}".format(self.object_list))
self.object_initial_position = Pose(position=Point(x=-0.13,
y=0.848,
z=1.06),
orientation=quaternion_from_euler(
0.002567, 0.102, 1.563))
# select first object, set object name and object type
# if object is random, spawn random object
# else get the first entry of object_list
self.set_target_object([0, 0, 0, 0, 0, 0])
# get maximum distance to the object to calculate reward, renewed in the reset function
self.max_distance, _ = U.get_distance_gripper_to_object()
# The closest distance during training
self.min_distance = 999
# get samples from reaching task
if self._load_init_pos:
import environments
self.init_samples = U.load_samples_from_prev_task(
os.path.dirname(environments.__file__) +
"/contacts_sample/door_sample/success_exp2019-05-21_11h41min.csv"
)
def __init__(self,
joint_increment=None,
sim_time_factor=0.001,
running_step=0.001,
random_object=False,
random_position=False,
use_object_type=False,
populate_object=False,
env_object_type='free_shapes'):
"""
initializing all the relevant variables and connections
:param joint_increment: increment of the joints
:param running_step: gazebo simulation time factor
:param random_object: spawn random object in the simulation
:param random_position: change object position in each reset
:param use_object_type: assign IDs to objects and used them in the observation space
:param populate_object: to populate object(s) in the simulation using sdf file
:param env_object_type: object type for environment, free_shapes for boxes while others are related to use_case
'door_handle', 'combox', ...
"""
# Assign Parameters
self._joint_increment = joint_increment
self.running_step = running_step
self._random_object = random_object
self._random_position = random_position
self._use_object_type = use_object_type
self._populate_object = populate_object
# Assign MsgTypes
self.joints_state = JointState()
self.contact_1_state = ContactsState()
self.contact_2_state = ContactsState()
self.collisions = Bool()
self.camera_rgb_state = Image()
self.camera_depth_state = Image()
self.contact_1_force = Vector3()
self.contact_2_force = Vector3()
self.gripper_state = VacuumGripperState()
self._list_of_observations = [
"distance_gripper_to_object", "elbow_joint_state",
"shoulder_lift_joint_state", "shoulder_pan_joint_state",
"wrist_1_joint_state", "wrist_2_joint_state",
"wrist_3_joint_state", "contact_1_force", "contact_2_force",
"object_pos_x", "object_pos_y", "object_pos_z",
"min_distance_gripper_to_object"
]
if self._use_object_type:
self._list_of_observations.append("object_type")
# Establishes connection with simulator
"""
1) Gazebo Connection
2) Controller Connection
3) Joint Publisher
"""
self.gazebo = GazeboConnection(sim_time_factor=sim_time_factor)
self.controllers_object = ControllersConnection()
self.pickbot_joint_pubisher_object = JointPub()
# Define Subscribers as Sensordata
"""
1) /pickbot/joint_states
2) /gripper_contactsensor_1_state
3) /gripper_contactsensor_2_state
4) /gz_collisions
not used so far but available in the environment
5) /pickbot/gripper/state
6) /camera_rgb/image_raw
7) /camera_depth/depth/image_raw
"""
rospy.Subscriber("/pickbot/joint_states", JointState,
self.joints_state_callback)
rospy.Subscriber("/gripper_contactsensor_1_state", ContactsState,
self.contact_1_callback)
rospy.Subscriber("/gripper_contactsensor_2_state", ContactsState,
self.contact_2_callback)
rospy.Subscriber("/gz_collisions", Bool, self.collision_callback)
# rospy.Subscriber("/pickbot/gripper/state", VacuumGripperState, self.gripper_state_callback)
# rospy.Subscriber("/camera_rgb/image_raw", Image, self.camera_rgb_callback)
# rospy.Subscriber("/camera_depth/depth/image_raw", Image, self.camera_depth_callback)
# Define Action and state Space and Reward Range
"""
Action Space: Box Space with 6 values.
State Space: Box Space with 12 values. It is a numpy array with shape (12,)
Reward Range: -infinity to infinity
"""
# Directly use joint_positions as action
if self._joint_increment is None:
low_action = np.array([
-(math.pi - 0.05), -(math.pi - 0.05), -(math.pi - 0.05),
-(math.pi - 0.05), -(math.pi - 0.05), -(math.pi - 0.05)
])
high_action = np.array([
math.pi - 0.05, math.pi - 0.05, math.pi - 0.05, math.pi - 0.05,
math.pi - 0.05, math.pi - 0.05
])
else: # Use joint_increments as action
low_action = np.array([
-self._joint_increment, -self._joint_increment,
-self._joint_increment, -self._joint_increment,
-self._joint_increment, -self._joint_increment
])
high_action = np.array([
self._joint_increment, self._joint_increment,
self._joint_increment, self._joint_increment,
self._joint_increment, self._joint_increment
])
self.action_space = spaces.Box(low_action, high_action)
high = np.array([
999, # distance_gripper_to_object
math.pi, # elbow_joint_state
math.pi, # shoulder_lift_joint_state
math.pi, # shoulder_pan_joint_state
math.pi, # wrist_1_joint_state
math.pi, # wrist_2_joint_state
math.pi, # wrist_3_joint_state
np.finfo(np.float32).max, # contact_1_force
np.finfo(np.float32).max, # contact_2_force
1, # object_pos_x
1.4, # object_pos_y
1.5, # object_pos_z
999
]) # min_distance_gripper_to_object
low = np.array([
0, # distance_gripper_to_object
-math.pi, # elbow_joint_state
-math.pi, # shoulder_lift_joint_state
-math.pi, # shoulder_pan_joint_state
-math.pi, # wrist_1_joint_state
-math.pi, # wrist_2_joint_state
-math.pi, # wrist_3_joint_state
0, # contact_1_force
0, # contact_2_force
-1, # object_pos_x
0, # object_pos_y
0, # object_pos_z
0
]) # min distance
if self._use_object_type:
high = np.append(high, 9)
low = np.append(low, 0)
self.observation_space = spaces.Box(low, high)
self.reward_range = (-np.inf, np.inf)
self._seed()
self.done_reward = 0
# set up everything to publish the Episode Number and Episode Reward on a rostopic
self.episode_num = 0
self.accumulated_episode_reward = 0
self.episode_steps = 0
self.reward_pub = rospy.Publisher('/openai/reward',
RLExperimentInfo,
queue_size=1)
self.reward_list = []
self.episode_list = []
self.step_list = []
self.csv_name = logger.get_dir() + '/result_log'
print("CSV NAME")
print(self.csv_name)
self.csv_success_exp = logger.get_dir(
) + "/success_exp" + datetime.datetime.now().strftime(
'%Y-%m-%d_%Hh%Mmin') + ".csv"
self.success_2_contact = 0
self.success_1_contact = 0
# object name: name of the target object
# object type: index of the object name in the object list
# object list: pool of the available objects, have at least one entry
self.object_name = ''
self.object_type_str = ''
self.object_type = 0
self.object_list = U.get_target_object(env_object_type)
print("object list {}".format(self.object_list))
self.object_initial_position = Pose(
position=Point(x=-0.13, y=0.848, z=1.06), # x=0.0, y=0.9, z=1.05
orientation=quaternion_from_euler(0.002567, 0.102, 1.563))
self.pickbot_initial_position = Pose(position=Point(x=0.0,
y=0.0,
z=1.12),
orientation=Quaternion(x=0.0,
y=0.0,
z=0.0,
w=1.0))
if self._populate_object:
# populate objects from object list
self.populate_objects()
# select first object, set object name and object type
# if object is random, spawn random object
# else get the first entry of object_list
self.set_target_object(random_object=self._random_object,
random_position=self._random_position)
# The distance between gripper and object, when the arm is in initial pose
self.max_distance, _ = U.get_distance_gripper_to_object()
# The minimum distance between gripper and object during training
self.min_distance = 999
class Controller:
rospy.init_node(node_name, anonymous=True)
ns = rospy.get_namespace()
Hz = 50
rate = rospy.Rate(Hz)
ns = rospy.get_namespace()
if ns == '/':
topic_states = model_name + '/link_states'
else:
topic_states = 'link_states'
soil_type = {"dry_sand": 0, "wet_sand": 1} #, "garbel": 2}
soil = "wet_sand"
K0 = 40 # penetration resistance of material
density = 1922 # density of material in kg/m^3
matirial_gravity = 2082 # specific gravity of material g/cm^3
## tool parameters
S = 0.04342
## definitions
linear_velocity_multiplier = 30
angular_velocity_multiplier = 10
force_multiplier = 1e3
res_wrench = Wrench()
res_wrench.force.x = 0
res_wrench.force.y = 0
res_wrench.force.z = 0
res_wrench.torque.x = 0
res_wrench.torque.y = 0
res_wrench.torque.z = 0
loader_pos = Pose()
contacts = ContactsState()
orientation_q = Quaternion()
force_on_bobcat = 0
joy_val = 0
x=0
z_collision = 0
volume = 0
volume_sum=0
last_z_pile = 0
last_x_step = 0
last_z_collision = 0
depth = 0
angular_vel = 0
m = (2.1213)/(1.9213 + 0.2) # the slope of the pile
z_collision = 0
roll = pitch = yaw = 0
def get_depth(self, data):
if (ContactsState.states != []):
i=0
for i in range(len(data.states)):
if ('box' in data.states[i].collision2_name) or ('box' in data.states[i].collision1_name): # check that the string exist in collision2_name/1
self.depth = np.mean(data.states[i].depths)
self.z_collision = np.mean(data.states[i].contact_positions[0].z)
def get_angular_vel(self, msg):
self.angular_vel = msg.angular_velocity.y
orientation_q = msg.orientation
orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
(self.roll, self.pitch, self.yaw) = euler_from_quaternion (orientation_list)
def get_linear_vel(self, msg):
lin = msg.linear_velocity
ang = msg.angular_velocity
self.force_on_bobcat = 4* self.force_multiplier * (self.linear_velocity_multiplier * lin +
self.angular_velocity_multiplier * ang)
def get_joy(self, msg):
self.joy_val = msg.linear_velocity
def __init__(self):
rospy.wait_for_service('/gazebo/get_model_state')
self.get_model_state = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)
self.get_link_state = rospy.ServiceProxy("/gazebo/get_link_state", GetLinkState)
self.apply_body_wrench = rospy.ServiceProxy('/gazebo/apply_body_wrench', ApplyBodyWrench)
self.pub = rospy.Publisher('/response', Wrench, queue_size=10)
self.pub2 = rospy.Publisher('/soil_type', Int32, queue_size=10)
self.pub3 = rospy.Publisher('/system_mode', Int32, queue_size=10)
rospy.Subscriber('/robot_bumper', ContactsState, self.get_depth)
rospy.Subscriber('/Bobby/imu', Imu, self.get_angular_vel)
rospy.Subscriber('/joy', Joy, self.get_joy)
self.pub3.publish(0)
while not rospy.is_shutdown():
self.pub2.publish(self.soil_type[self.soil])
rospy.wait_for_service('/gazebo/get_link_state')
self.loader_pos = self.get_link_state('Bobby::loader', 'world').link_state.pose
self.body_pos = self.get_link_state('Bobby::body', 'world').link_state.pose
z_pile = self.m * (self.loader_pos.position.x + 0.96 * math.cos(self.pitch) + 0.2) # 0.96 is the distance between center mass of the loader to the end
H = z_pile - self.z_collision
if self.depth > 0.001 :
z_pile = self.m * (self.loader_pos.position.x + 0.96 * math.cos(
self.pitch) + 0.2) # 0.96 is the distance between center mass of the loader to the end
x = self.loader_pos.position.x + 0.96 * math.cos(self.pitch)
big_trapezoid = (x - self.last_x_step) * (z_pile + self.last_z_pile) / 2
small_trapezoid = (x - self.last_x_step) * (self.z_collision + self.last_z_collision) / 2
volume = (big_trapezoid - small_trapezoid) * 1.66 * self.density # 1.66 is the tool width
if z_pile > 0 and self.z_collision > 0 and z_pile > self.z_collision:
self.volume_sum = self.volume_sum + volume
self.last_z_pile = z_pile
self.last_x_step = x
self.last_z_collision = self.z_collision
F2 = self.K0 * math.cos(self.angular_vel) * self.matirial_gravity * H * self.S * 9.81
self.res_wrench.force.x = -(F2 * math.cos(self.pitch))
self.res_wrench.force.z = -(volume * 9.81 + F2 * math.sin(self.pitch))
# build data for the learning algorithm
if self.depth <= 0.001:
self.res_wrench.force.x = 0
self.res_wrench.force.z = 0
rospy.wait_for_service('/gazebo/apply_body_wrench')
try:
self.apply_body_wrench(body_name=body_name,
reference_frame="",
wrench=self.res_wrench,
start_time=rospy.Time.from_sec(0),
duration=rospy.Duration.from_sec(1.0))
except Exception as e:
print("/gazebo/reset_simulation service call failed")
self.pub.publish(self.res_wrench)
self.rate.sleep()
def reset(self, spec=None):
self.laser_data = LaserScan()
self.contact_data = ContactsState()
# Resets the simulation
rospy.wait_for_service('/gazebo/reset_world')
try:
self.reset_world()
except rospy.ServiceException as e:
print("# Reset simulation fails!")
# Set robot to random starting point
if spec is not None:
pose = Pose()
pose.position.x = spec[0]
pose.position.y = spec[1]
pose.position.z = self.get_model_state(
model_name="mobile_base").pose.position.z
theta = spec[2]
ox, oy, oz, ow = quaternion_from_euler(0.0, 0.0, theta)
pose.orientation.x = ox
pose.orientation.y = oy
pose.orientation.z = oz
pose.orientation.w = ow
else:
pose = Pose()
pose.position.x = np.random.uniform(low=START_STATE[0] - 0.5,
high=START_STATE[0] + 0.5)
pose.position.y = np.random.uniform(low=START_STATE[1] - 0.5,
high=START_STATE[1] + 0.5)
pose.position.z = self.get_model_state(
model_name="mobile_base").pose.position.z
theta = np.random.uniform(low=START_STATE[2],
high=START_STATE[2] + np.pi)
ox, oy, oz, ow = quaternion_from_euler(0.0, 0.0, theta)
pose.orientation.x = ox
pose.orientation.y = oy
pose.orientation.z = oz
pose.orientation.w = ow
reset_state = ModelState()
reset_state.model_name = "mobile_base"
reset_state.pose = pose
self.set_model_state(reset_state)
# Prepare receive sensor readings
laser_data = self.get_laser()
new_laser_data = laser_data
# Unpause simulation only for obtaining observation
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except rospy.ServiceException as e:
print("/gazebo/unpause_physics service call failed")
while new_laser_data.header.stamp <= laser_data.header.stamp:
new_laser_data = self.get_laser()
# Pause simulator to do other operations
rospy.wait_for_service('/gazebo/pause_physics')
try:
self.pause()
except rospy.ServiceException as e:
print("/gazebo/pause_physics service call failed")
# read dynamics data
dynamic_data = None
rospy.wait_for_service("/gazebo/get_model_state")
while dynamic_data is None:
dynamic_data = self.get_model_state(model_name="mobile_base")
obsrv = self.get_obsrv(new_laser_data, dynamic_data)
self.pre_obsrv = obsrv
return np.asarray(obsrv)
#!/usr/bin/env python3
import rospy
from gazebo_msgs.msg import ContactsState
from my_robot_world.srv import *
contact_state = ContactsState()
def callback_sensor(data):
global contact_state
contact_state = data.states
def check_sensor(req):
global contact_state
object_detected = False
if not contact_state:
object_detected = False
model_name = ""
else:
object_detected = True
subs = str(contact_state[0].collision1_name)
trim = subs.partition('::')
model_name = trim[0]
return object_detected, model_name
def main():
rospy.init_node("contact_sensor_service")
rospy.Subscriber("/sensor_contact", ContactsState, callback_sensor)