def get_joint_position(self): '''Get joint position using streaming communication.''' joint_pos = [ vu.get_joint_position(self.client_id, h) for h in self.handles_dict["joint"] ] return joint_pos
def get_handles(self): client_id = self.client_id _handle_by_name = lambda n: vu.get_handle_by_name(client_id, n) joint_names = [ "Prismatic_joint_X", "Prismatic_joint_Y", "Prismatic_joint_Z", "Revolute_joint_Z", "Revolute_joint_Y", "Revolute_joint_X", # "Prismatic_joint_Fing1", # "Prismatic_joint_Fing2" ] joint_handles = [_handle_by_name(name) for name in joint_names] ft_names = ["FT_sensor_EE"] ft_handles = [_handle_by_name(n) for n in ft_names] ''' ee_names = ["EndEffector"] ee_handles = [_handle_by_name(n) for n in ee_names] ft_names = ["FT_sensor_Wrist", "FT_sensor_EE"] ft_handles = [_handle_by_name(n) for n in ft_names] cam_rgb_names = ["PalmCam_RGB"] cam_rgb_handles =[_handle_by_name(n) for n in cam_rgb_names] cam_depth_names = ["PalmCam_Depth"] cam_depth_handles =[_handle_by_name(n) for n in cam_depth_names] ''' robot_handles_dict = { "joint": joint_handles, "ft": ft_handles, # "EE": ee_handles, # "FT":ft_handles, # "RGB":cam_rgb_handles, # "Depth":cam_depth_handles } ## Start Data Streaming joint_pos = [ vu.get_joint_position(client_id, h) for h in joint_handles ] ''' EEPose=vrep.simxGetObjectPosition(clientID,RobotHandles["EE"][0],-1,vrep.simx_opmode_streaming)[1]; EEPose.extend(vrep.simxGetObjectOrientation(clientID,RobotHandles["EE"][0],-1,vrep.simx_opmode_streaming)[1]); FTVal=[vrep.simxReadForceSensor(clientID,FTHandle,vrep.simx_opmode_streaming)[2:4] for FTHandle in RobotHandles["FT"]]; RobotVisionRGB=[vrep.simxGetVisionSensorImage(clientID,sensorHandle,0,vrep.simx_opmode_streaming) for sensorHandle in RobotHandles["RGB"]]; RobotVisionDepth=[vrep.simxGetVisionSensorDepthBuffer(clientID,sensorHandle,vrep.simx_opmode_streaming) for sensorHandle in RobotHandles["Depth"]]; ''' return robot_handles_dict
def get_handles(self): client_id = self.client_id _handle_by_name = lambda n: vu.get_handle_by_name(client_id, n) joint_names = [ "Prismatic_joint_X", "Prismatic_joint_Y", "Prismatic_joint_Z", "Revolute_joint_Z", "Revolute_joint_Y", "Revolute_joint_X", # "Prismatic_joint_Fing1", # "Prismatic_joint_Fing2" ] joint_handles = [_handle_by_name(name) for name in joint_names] ft_names = ["FT_sensor_EE"] ft_handles = [_handle_by_name(n) for n in ft_names] octree_handle = _handle_by_name("Octree") # vision_sensor = _handle_by_name("Vision_sensor") robot_handles_dict = dict( joint=joint_handles, ft=ft_handles, octree=octree_handle, # vision_sensor=vision_sensor, # "EE": ee_handles, # "FT":ft_handles, # "RGB":cam_rgb_handles, # "Depth":cam_depth_handles ) ## Start Data Streaming joint_pos = [ vu.get_joint_position(client_id, h) for h in joint_handles ] return robot_handles_dict
def run_scene(self, scene_i, save_dir): self.scene_info = {} args = self.args logging.info("Start sim: {}".format(scene_i)) self.reset() self.step() # Generate anchor handle anchor_pos = [0.0, 0.0, 0.0] anchor_args = self.get_anchor_object_args() anchor_handle = self.generate_object_with_args(anchor_args) for _ in range(4): self.step() # Generate the other object other_pos = [0.0, 0.0, 0.0] other_obj_args = self.get_other_object_args( other_pos, obj_size=[0.04, 0.04, 0.04], add_obj_z=False) other_handle = self.generate_object_with_args(other_obj_args) for _ in range(5): self.step() # Move other obj to the right place anchor_abs_bb = self.get_absolute_bb(anchor_handle) anchor_lb, anchor_ub = anchor_abs_bb[0], anchor_abs_bb[1] # other_obj_pos = self.get_other_obj_position( # anchor_args, anchor_pos, anchor_lb, anchor_ub, # other_obj_args['obj_size'] # ) new_other_obj_pos = self.get_other_obj_location_outside_anchor_cuboid( anchor_args, anchor_pos, anchor_lb, anchor_ub, other_obj_args['obj_size']) if new_other_obj_pos is None: logging.info("Cannot place other obj near anchor.") return print("Should move other obj to: {}".format( _array_to_str(new_other_obj_pos))) anchor_pos = self.get_object_position(anchor_handle, -1) logging.info("Anchor pos: {}".format(_array_to_str(anchor_pos))) joint_handles = self.handles_dict['joint'] joint_pos = [ vu.get_joint_position(self.client_id, h) for h in self.handles_dict["joint"] ] logging.info("Current joint pos: {}".format(_array_to_str(joint_pos))) self.set_all_joint_positions(new_other_obj_pos + [0, 0, 0]) for _ in range(10): self.step() joint_pos = [ vu.get_joint_position_oneshot(self.client_id, h) for h in self.handles_dict["joint"] ] logging.info("New joint pos: {}".format(_array_to_str(joint_pos))) old_pid_values = self.get_joint_pid_values() # self.set_joint_pid_values(10.0, 0.0, 1.0) ''' self.set_diff_joint_pid_values([ [0.01, 0, 0], +1.0000e-01 [0.01, 0, 0], [0.01, 0, 0], [0.05, 0, 0], [0.05, 0, 0], [0.05, 0, 0], ]) ''' new_pid_values = self.get_joint_pid_values() logging.info("Old pid values: {}\n" "new pid values: {}\n".format( _array_to_str(old_pid_values), _array_to_str(new_pid_values))) # Now try to perform actions # Try to go down for now if TEST_MOVE_IN_DIR == 0: after_action_joint_pos = [ joint_pos[0], joint_pos[1], anchor_pos[2] ] elif TEST_MOVE_IN_DIR == 1: after_action_joint_pos = [ joint_pos[0], anchor_pos[1], joint_pos[2] ] elif TEST_MOVE_IN_DIR == 2: after_action_joint_pos = [ anchor_pos[0], joint_pos[1], joint_pos[2] ] self.toggle_record_ft_data() self.step() self.set_prismatic_joints(after_action_joint_pos) for _ in range(5): joint_pos = self.get_joint_position() print(_array_to_str(joint_pos)) self.step() ''' self.free_other_object() self.set_joint_pid_values(0.1, 0.0, 0.0) for _ in range(20): self.step() curr_joint_pos = self.get_joint_position() self.reattach_other_object() self.set_all_joint_positions(curr_joint_pos) ''' for i in range(50): self.step() if i % 5 == 0: joint_pos = self.get_joint_position() print(_array_to_str(joint_pos)) ft_data = self.save_record_ft_data() assert ft_data is not None ft_data_arr = np.array(ft_data).reshape(-1, 6) ft_mean = np.mean(ft_data_arr, axis=0) logging.info( bcolors.c_red("Mean ft value: {}".format(_array_to_str(ft_mean)))) return True
def run_scene(self, scene_i, save_dir): self.scene_info = {} args = self.args logging.info("Start sim: {}".format(scene_i)) self.reset() self.step() # Generate anchor handle anchor_pos = [0, 0, 0.1] anchor_args = self.get_anchor_object_args() anchor_handle = self.generate_object_with_args(anchor_args) for _ in range(4): self.step() # Generate the other object other_pos = [0, -0.1, 0.0] other_obj_args = self.get_other_object_args(other_pos, add_obj_z=False) other_handle = self.generate_object_with_args(other_obj_args) for _ in range(5): self.step() # Move other obj to the right place anchor_abs_bb = self.get_absolute_bb(anchor_handle) anchor_lb, anchor_ub = anchor_abs_bb[0], anchor_abs_bb[1] # other_obj_pos = self.get_other_obj_position( # anchor_args, anchor_pos, anchor_lb, anchor_ub, # other_obj_args['obj_size'] # ) new_other_obj_pos = self.get_other_obj_location_outside_anchor_cuboid( anchor_args, anchor_pos, anchor_lb, anchor_ub, other_obj_args['obj_size']) if new_other_obj_pos is None: logging.info("Cannot place other obj near anchor.") return print("Should move other obj to: {}".format( _array_to_str(new_other_obj_pos))) anchor_pos = self.get_object_position(anchor_handle, -1) logging.info("Anchor pos: {}".format(_array_to_str(anchor_pos))) joint_handles = self.handles_dict['joint'] joint_pos = [ vu.get_joint_position(self.client_id, h) for h in self.handles_dict["joint"] ] logging.info("Current joint pos: {}".format(_array_to_str(joint_pos))) self.set_all_joint_positions(new_other_obj_pos + [0, 0, 0]) for _ in range(10): self.step() joint_pos = [ vu.get_joint_position(self.client_id, h) for h in self.handles_dict["joint"] ] logging.info("New joint pos: {}".format(_array_to_str(joint_pos))) old_pid_values = self.get_joint_pid_values() self.set_joint_pid_values(10.0, 0.0, 1.0) new_pid_values = self.get_joint_pid_values() logging.info("Old pid values: {}\n" "new pid values: {}\n".format( _array_to_str(old_pid_values), _array_to_str(new_pid_values))) # Now try to perform actions # Try to go down for now after_action_joint_pos = [ joint_pos[0], joint_pos[1], anchor_pos[2], 0, 0, 0 ] self.set_all_joint_positions(after_action_joint_pos) for _ in range(5): self.step() self.free_other_object() self.set_joint_pid_values(0.1, 0.0, 0.0) for _ in range(20): self.step() curr_joint_pos = self.get_joint_position() self.reattach_other_object() self.set_all_joint_positions(curr_joint_pos) ''' self.step() joint_pos[2] += 0.2 self.set_all_joint_positions(joint_pos) other_pos = self.get_object_position(other_handle, -1) logging.info("Other pos: ({:.4f}, {:.4f}, {:.4f})".format( other_pos[0], other_pos[1], other_pos[2])) # Move to slightly above the anchor block. offset = np.array([0.0, 0.0, 0.00]) # above_anchor_pos = anchor_pos - other_pos + offset above_anchor_pos = anchor_pos + offset logging.info("Move to pos: ({}, {}, {})".format(above_anchor_pos[0], above_anchor_pos[1], above_anchor_pos[2])) self.set_all_joint_positions(above_anchor_pos.tolist() + [0, 0, 0]) self.step() # self.set_object_position(other_handle, above_anchor_pos.tolist()) for _ in range(10): False self.step() joint_pos = [vu.get_joint_position(self.client_id, h) for h in self.handles_dict["joint"]] logging.info("Joint pos: {}".format(joint_pos)) # self.set_object_position(other_handle, anchor_pos.tolist()) # self.set_object_position(self.handles_dict['ft'][0], anchor_pos) ''' for _ in range(50): self.step() return True
def get_joint_position(self): joint_pos = [ vu.get_joint_position(self.client_id, h) for h in self.handles_dict["joint"] ] return joint_pos