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)