def main(): parser = argparse.ArgumentParser() parser.add_argument('dir', type=pathlib.Path) args = parser.parse_args() rospy.init_node('filter_scenes') link_states_listener = Listener("/gazebo/link_states", LinkStates) bagfile_names = list(args.dir.iterdir()) bagfile_names = sorted(bagfile_names) for bagfile_name in bagfile_names: if bagfile_name.suffix == '.bag': index = int(bagfile_name.stem[-4:]) srv_name = "gazebo/set_link_state" rospy.loginfo(f"waiting for service {srv_name}") set_srv = rospy.ServiceProxy(srv_name, SetLinkState) set_srv.wait_for_service() gazebo_service_provider = GazeboServices() gazebo_service_provider.restore_from_bag(bagfile_name) gazebo_service_provider.play() r = input(f"{index:4d}: fix? [N/y]") if r == 'Y' or r == 'y': # let the use fix it input("press enter when you're done fixing") links_states = link_states_listener.get() bagfile_name = args.dir / f'scene_{index:04d}.bag' rospy.loginfo(f"Saving scene to {bagfile_name}") with rosbag.Bag(bagfile_name, 'w') as bag: bag.write('links_states', links_states)
def __init__(self, robot_namespace: str = ''): """ This class is designed around the needs of the trajectory_follower.TrajectoryFollower This class really only contains API that is needed by that class. The trajectory follower only needs to know about the basic ROS API -- how to send and get joint commands. However, because MoveIt trajectory execution relies (for now) on that TrajectoryFollower class, we do not want BaseRobot to use any MoveIt TrajectoryExeuction or rely on any trajectory execution services. """ self.robot_namespace = robot_namespace # the robot namespace will be prepended by setting ROS_NAMESPACE environment variable or the ns="" in roslaunch joint_states_topic = ns_join(self.robot_namespace, 'joint_states') self.joint_state_listener = Listener(joint_states_topic, JointState) # NOTE: derived classes must set these values self.right_gripper_command_pub = None self.left_gripper_command_pub = None self.tf_wrapper = TF2Wrapper() try: self.robot_commander = moveit_commander.RobotCommander( ns=self.robot_namespace) except RuntimeError as e: rospy.logerr( "You may need to load the moveit planning context and robot description" ) print(e)
def __init__(self): self.output_throttle_period = 5.0 self.gripper_status = \ {"right": Listener("right_arm/gripper_status", Robotiq3FingerStatus), "left": Listener("left_arm/gripper_status", Robotiq3FingerStatus)} self.gripper_command_publisher = \ {"right": rospy.Publisher("right_arm/gripper_command", Robotiq3FingerCommand, queue_size=1), "left": rospy.Publisher("left_arm/gripper_command", Robotiq3FingerCommand, queue_size=1)} self.joy_sub = rospy.Subscriber("joy", Joy, self.joy_callback) self.prev_xbox_msg = None
def generate_test_scenes( scenario: str, n_trials: int, params_filename: pathlib.Path, save_test_scenes_dir: Optional[pathlib.Path] = None, ): service_provider = gazebo_services.GazeboServices() scenario = get_scenario(scenario) service_provider.setup_env(verbose=0, real_time_rate=0.0, max_step_size=0.01, play=True) link_states_listener = Listener("gazebo/link_states", LinkStates) env_rng = np.random.RandomState(0) action_rng = np.random.RandomState(0) with params_filename.open("r") as params_file: params = hjson.load(params_file) scenario.on_before_data_collection(params) scenario.randomization_initialization() for trial_idx in range(n_trials): environment = scenario.get_environment(params) scenario.randomize_environment(env_rng, params) for i in range(10): state = scenario.get_state() action = scenario.sample_action( action_rng=action_rng, environment=environment, state=state, action_params=params, validate=True, ) scenario.execute_action(action) links_states = link_states_listener.get() save_test_scenes_dir.mkdir(exist_ok=True, parents=True) bagfile_name = save_test_scenes_dir / f'scene_{trial_idx:04d}.bag' rospy.loginfo(f"Saving scene to {bagfile_name}") with rosbag.Bag(bagfile_name, 'w') as bag: bag.write('links_states', links_states)
def __init__(self, robot_namespace: str): DualArmRobot.__init__(self, robot_namespace=robot_namespace) self.left_arm_command_pub = rospy.Publisher( self.ns("left_arm/motion_command"), MotionCommand, queue_size=10) self.right_arm_command_pub = rospy.Publisher( self.ns("right_arm/motion_command"), MotionCommand, queue_size=10) self.left_gripper_command_pub = rospy.Publisher( self.ns("left_arm/gripper_command"), Robotiq3FingerCommand, queue_size=10) self.right_gripper_command_pub = rospy.Publisher( self.ns("right_arm/gripper_command"), Robotiq3FingerCommand, queue_size=10) self.left_set_control_mode_srv = rospy.ServiceProxy( self.ns("left_arm/set_control_mode_service"), SetControlMode) self.right_set_control_mode_srv = rospy.ServiceProxy( self.ns("right_arm/set_control_mode_service"), SetControlMode) self.left_get_control_mode_srv = rospy.ServiceProxy( self.ns("left_arm/get_control_mode_service"), GetControlMode) self.right_get_control_mode_srv = rospy.ServiceProxy( self.ns("right_arm/get_control_mode_service"), GetControlMode) self.left_arm_status_listener = Listener( self.ns("left_arm/motion_status"), MotionStatus) self.right_arm_status_listener = Listener( self.ns("right_arm/motion_status"), MotionStatus) self.left_gripper_status_listener = Listener( self.ns("left_arm/gripper_status"), Robotiq3FingerStatus) self.right_gripper_status_listener = Listener( self.ns("right_arm/gripper_status"), Robotiq3FingerStatus) self.waypoint_state_pub = rospy.Publisher( self.ns("waypoint_robot_state"), DisplayRobotState, queue_size=10)
def __init__(self, robot_namespace): super().__init__() self.robot_namespace = robot_namespace self.service_provider = BaseServices() self.joint_state_viz_pub = rospy.Publisher(ns_join( self.robot_namespace, "joint_states_viz"), JointState, queue_size=10) self.goto_home_srv = rospy.ServiceProxy("goto_home", Empty) self.cdcpd_listener = Listener("cdcpd/output", PointCloud2) self.cdcpd_reset_srv = rospy.ServiceProxy("cdcpd/reset", Empty) self.attach_srv = rospy.ServiceProxy("/link_attacher_node/attach", Attach) self.detach_srv = rospy.ServiceProxy("/link_attacher_node/detach", Attach) exclude_srv_name = ns_join(self.robot_namespace, "exclude_models_from_planning_scene") self.exclude_from_planning_scene_srv = rospy.ServiceProxy( exclude_srv_name, ExcludeModels) # FIXME: this blocks until the robot is available, we need lazy construction self.robot = get_moveit_robot(self.robot_namespace)
def __init__(self): super().__init__() self.color_image_listener = Listener(self.COLOR_IMAGE_TOPIC, Image) self.depth_image_listener = Listener(self.DEPTH_IMAGE_TOPIC, Image) self.state_color_viz_pub = rospy.Publisher("state_color_viz", Image, queue_size=10, latch=True) self.state_depth_viz_pub = rospy.Publisher("state_depth_viz", Image, queue_size=10, latch=True) self.last_action = None self.get_rope_end_points_srv = rospy.ServiceProxy( ns_join(self.ROPE_NAMESPACE, "get_dual_gripper_points"), GetDualGripperPoints) self.get_rope_srv = rospy.ServiceProxy( ns_join(self.ROPE_NAMESPACE, "get_rope_state"), GetRopeState) self.pos3d = Position3D() self.cdcpd_listener = Listener("cdcpd/output", PointCloud2) self.set_rope_state_srv = rospy.ServiceProxy( ns_join(self.ROPE_NAMESPACE, "set_rope_state"), SetRopeState) self.reset_srv = rospy.ServiceProxy("/gazebo/reset_simulation", Empty) try: self.left_gripper_bbox_pub = rospy.Publisher( '/left_gripper_bbox_pub', BoundingBox, queue_size=10, latch=True) self.right_gripper_bbox_pub = rospy.Publisher( '/right_gripper_bbox_pub', BoundingBox, queue_size=10, latch=True) except NameError: pass self.overstretching_srv = rospy.ServiceProxy( ns_join(self.ROPE_NAMESPACE, "rope_overstretched"), GetOverstretching) self.error_pub = rospy.Publisher("error", Float32, queue_size=10) self.max_action_attempts = 100 self.robot_reset_rng = np.random.RandomState(0)
class DualArmRobot: def __init__(self, robot_namespace: str = ''): """ This class is designed around the needs of the trajectory_follower.TrajectoryFollower This class really only contains API that is needed by that class. The trajectory follower only needs to know about the basic ROS API -- how to send and get joint commands. However, because MoveIt trajectory execution relies (for now) on that TrajectoryFollower class, we do not want BaseRobot to use any MoveIt TrajectoryExeuction or rely on any trajectory execution services. """ self.robot_namespace = robot_namespace # the robot namespace will be prepended by setting ROS_NAMESPACE environment variable or the ns="" in roslaunch joint_states_topic = ns_join(self.robot_namespace, 'joint_states') self.joint_state_listener = Listener(joint_states_topic, JointState) # NOTE: derived classes must set these values self.right_gripper_command_pub = None self.left_gripper_command_pub = None self.tf_wrapper = TF2Wrapper() try: self.robot_commander = moveit_commander.RobotCommander( ns=self.robot_namespace) except RuntimeError as e: rospy.logerr( "You may need to load the moveit planning context and robot description" ) print(e) def ns(self, name: str): return ns_join(self.robot_namespace, name) def send_joint_command( self, joint_names: List[str], trajectory_point: JointTrajectoryPoint) -> Tuple[bool, str]: raise NotImplementedError() def get_joint_positions(self, joint_names: Optional[List[str]] = None): """ :args joint_names an optional list of names if you want to have a specific order or a subset """ joint_state: JointState = self.joint_state_listener.get() if joint_names is None: return joint_state.position current_joint_positions = [] for name in joint_names: if name not in joint_state.name: ros_helpers.logfatal( ValueError, f"Joint {name} not found in joint states") idx = joint_state.name.index(name) pos = joint_state.position[idx] current_joint_positions.append(pos) return current_joint_positions def check_inputs(self, group_name: str, ee_link_name: str): links = self.robot_commander.get_link_names() if ee_link_name not in links: rospy.logwarn_throttle( 1, f"Link [{ee_link_name}] does not exist. Existing links are:") rospy.logwarn_throttle(1, links) groups = self.robot_commander.get_group_names() if group_name not in groups: rospy.logwarn_throttle( 1, f"Group [{group_name}] does not exist. Existing groups are:") rospy.logwarn_throttle(1, groups) def get_right_gripper_links(self): return self.robot_commander.get_link_names("right_gripper") def get_left_gripper_links(self): return self.robot_commander.get_link_names("left_gripper") def open_left_gripper(self): self.left_gripper_command_pub.publish(self.get_open_gripper_msg()) def open_right_gripper(self): self.right_gripper_command_pub.publish(self.get_open_gripper_msg()) def close_left_gripper(self): # TODO: implementing blocking grasping self.left_gripper_command_pub.publish(self.get_close_gripper_msg()) def close_right_gripper(self): self.right_gripper_command_pub.publish(self.get_close_gripper_msg()) def get_close_gripper_msg(self): raise NotImplementedError() def get_open_gripper_msg(self): raise NotImplementedError()
[str(np.round(rad, 3)) for rad in vec]) + ']' if (left is not None) and (left.last_pos is not None): rospy.loginfo("Left: " + vec_to_rad_str(left.last_pos)) if (right is not None) and (right.last_pos is not None): rospy.loginfo("Right: " + vec_to_rad_str(right.last_pos)) if __name__ == "__main__": rospy.init_node("manual_motion") control_mode_params = vu.get_joint_impedance_params(vu.Stiffness.MEDIUM) med = Med(robot_namespace="med") rospy.loginfo("intializing arm") r_cm = Listener("med/control_mode_status", ControlModeParameters) cur_mode = r_cm.get(block_until_data=True) control_mode_params.joint_path_execution_params.joint_relative_velocity = \ cur_mode.joint_path_execution_params.joint_relative_velocity control_mode_params.joint_path_execution_params.joint_relative_acceleration = \ cur_mode.joint_path_execution_params.joint_relative_acceleration sys.stdout.flush() result = vu.send_new_control_mode("med", control_mode_params) while not result.success: result = vu.send_new_control_mode("med", control_mode_params) print("done") med = ManualMotion("med") rospy.on_shutdown(lambda: print_joints(med, right=None)) rospy.loginfo("Running")
class FloatingRopeScenario(Base3DScenario): IMAGE_H = 90 IMAGE_W = 160 n_links = 25 KINECT_NAME = "kinect2" COLOR_IMAGE_TOPIC = ns_join(KINECT_NAME, "qhd/image_color_rect") DEPTH_IMAGE_TOPIC = ns_join(KINECT_NAME, "qhd/image_depth_rect") crop_region = { 'min_y': 0, 'min_x': 0, 'max_y': 540, 'max_x': 960, } ROPE_NAMESPACE = 'rope_3d' # TODO: break out the different pieces of get_state to make them composable, # since there are just a few shared amongst all the scenarios # TODO: about this... maybe they should all be pure functions? do we really need "self" at all? # the one reason we have classes at all is so that we can describe interfaces via type hints def __init__(self): super().__init__() self.color_image_listener = Listener(self.COLOR_IMAGE_TOPIC, Image) self.depth_image_listener = Listener(self.DEPTH_IMAGE_TOPIC, Image) self.state_color_viz_pub = rospy.Publisher("state_color_viz", Image, queue_size=10, latch=True) self.state_depth_viz_pub = rospy.Publisher("state_depth_viz", Image, queue_size=10, latch=True) self.last_action = None self.get_rope_end_points_srv = rospy.ServiceProxy( ns_join(self.ROPE_NAMESPACE, "get_dual_gripper_points"), GetDualGripperPoints) self.get_rope_srv = rospy.ServiceProxy( ns_join(self.ROPE_NAMESPACE, "get_rope_state"), GetRopeState) self.pos3d = Position3D() self.cdcpd_listener = Listener("cdcpd/output", PointCloud2) self.set_rope_state_srv = rospy.ServiceProxy( ns_join(self.ROPE_NAMESPACE, "set_rope_state"), SetRopeState) self.reset_srv = rospy.ServiceProxy("/gazebo/reset_simulation", Empty) try: self.left_gripper_bbox_pub = rospy.Publisher( '/left_gripper_bbox_pub', BoundingBox, queue_size=10, latch=True) self.right_gripper_bbox_pub = rospy.Publisher( '/right_gripper_bbox_pub', BoundingBox, queue_size=10, latch=True) except NameError: pass self.overstretching_srv = rospy.ServiceProxy( ns_join(self.ROPE_NAMESPACE, "rope_overstretched"), GetOverstretching) self.error_pub = rospy.Publisher("error", Float32, queue_size=10) self.max_action_attempts = 100 self.robot_reset_rng = np.random.RandomState(0) def needs_reset(self): res: GetOverstretchingResponse = self.overstretching_srv( GetOverstretchingRequest()) return res.magnitude > 1.30 def trajopt_distance_to_goal_differentiable(self, final_state, goal_state: Dict): return self.cfm_distance(final_state['z'], goal_state['z']) def trajopt_distance_differentiable(self, s1, s2): return self.cfm_distance(s1['z'], s2['z']) def cfm_distance(self, z1, z2): return tf.math.reduce_sum(tf.math.square(z1 - z2), axis=-1, keepdims=True) def get_environment(self, params: Dict, **kwargs): return {} def hard_reset(self): self.reset_srv(EmptyRequest()) def randomization_initialization(self): pass def on_before_action(self): self.register_fake_grasping() def on_before_data_collection(self, params: Dict): self.on_before_action() left_gripper_position = np.array([1.0, 0.2, 1.0]) right_gripper_position = np.array([1.0, -0.2, 1.0]) init_action = { 'left_gripper_position': left_gripper_position, 'right_gripper_position': right_gripper_position, } self.execute_action(init_action) def execute_action(self, action: Dict): speed_mps = action.get('speed', 0.1) left_req = Position3DActionRequest( speed_mps=speed_mps, scoped_link_name=gz_scope(self.ROPE_NAMESPACE, 'left_gripper'), position=ros_numpy.msgify(Point, action['left_gripper_position'])) right_req = Position3DActionRequest( speed_mps=speed_mps, scoped_link_name=gz_scope(self.ROPE_NAMESPACE, 'right_gripper'), position=ros_numpy.msgify(Point, action['right_gripper_position'])) self.pos3d.set(left_req) self.pos3d.set(right_req) wait_req = Position3DWaitRequest() wait_req.timeout_s = 10.0 wait_req.scoped_link_names.append( gz_scope(self.ROPE_NAMESPACE, 'left_gripper')) wait_req.scoped_link_names.append( gz_scope(self.ROPE_NAMESPACE, 'right_gripper')) self.pos3d.wait(wait_req) rope_settling_time = action.get('settling_time', 1.0) rospy.sleep(rope_settling_time) def reset_rope(self, action_params: Dict): reset = SetRopeStateRequest() # TODO: rename this to rope endpoints reset positions or something reset.left_gripper.x = numpify( action_params['left_gripper_reset_position'][0]) reset.left_gripper.y = numpify( action_params['left_gripper_reset_position'][1]) reset.left_gripper.z = numpify( action_params['left_gripper_reset_position'][2]) reset.right_gripper.x = numpify( action_params['right_gripper_reset_position'][0]) reset.right_gripper.y = numpify( action_params['right_gripper_reset_position'][1]) reset.right_gripper.z = numpify( action_params['right_gripper_reset_position'][2]) self.set_rope_state_srv(reset) def sample_action(self, action_rng: np.random.RandomState, environment: Dict, state: Dict, action_params: Dict, validate: bool, stateless: Optional[bool] = False): self.viz_action_sample_bbox( self.left_gripper_bbox_pub, self.get_action_sample_extent(action_params, 'left')) self.viz_action_sample_bbox( self.right_gripper_bbox_pub, self.get_action_sample_extent(action_params, 'right')) action = None for _ in range(self.max_action_attempts): # move in the same direction as the previous action with some probability repeat_probability = action_params[ 'repeat_delta_gripper_motion_probability'] if not stateless and self.last_action is not None and action_rng.uniform( 0, 1) < repeat_probability: left_gripper_delta_position = self.last_action[ 'left_gripper_delta_position'] right_gripper_delta_position = self.last_action[ 'right_gripper_delta_position'] else: # Sample a new random action left_gripper_delta_position = self.sample_delta_position( action_params, action_rng) right_gripper_delta_position = self.sample_delta_position( action_params, action_rng) # Apply delta and check for out of bounds left_gripper_position = state[ 'left_gripper'] + left_gripper_delta_position right_gripper_position = state[ 'right_gripper'] + right_gripper_delta_position action = { 'left_gripper_position': left_gripper_position, 'right_gripper_position': right_gripper_position, 'left_gripper_delta_position': left_gripper_delta_position, 'right_gripper_delta_position': right_gripper_delta_position, } if not validate or self.is_action_valid(action, action_params): self.last_action = action return action rospy.logwarn( "Could not find a valid action, executing an invalid one") return action def is_action_valid(self, action: Dict, action_params: Dict): out_of_bounds = FloatingRopeScenario.grippers_out_of_bounds( action['left_gripper_position'], action['right_gripper_position'], action_params) max_gripper_d = default_if_none( action_params['max_distance_between_grippers'], 1000) gripper_d = np.linalg.norm(action['left_gripper_position'] - action['right_gripper_position']) too_far = gripper_d > max_gripper_d return not out_of_bounds and not too_far def get_action_sample_extent(self, action_params: Dict, prefix: str): k = prefix + 'gripper_action_sample_extent' if k in action_params: gripper_extent = np.array(action_params[k]).reshape([3, 2]) else: gripper_extent = np.array(action_params['extent']).reshape([3, 2]) return gripper_extent def viz_action_sample_bbox(self, gripper_bbox_pub: rospy.Publisher, gripper_extent): gripper_bbox_msg = extent_array_to_bbox(gripper_extent) gripper_bbox_msg.header.frame_id = 'world' gripper_bbox_pub.publish(gripper_bbox_msg) def sample_delta_position(self, action_params, action_rng): pitch = action_rng.uniform(-np.pi, np.pi) yaw = action_rng.uniform(-np.pi, np.pi) displacement = action_rng.uniform( 0, action_params['max_distance_gripper_can_move']) rotation_matrix = transformations.euler_matrix(0, pitch, yaw) gripper_delta_position_homo = rotation_matrix @ np.array( [1, 0, 0, 1]) * displacement gripper_delta_position = gripper_delta_position_homo[:3] return gripper_delta_position @staticmethod def grippers_out_of_bounds(left_gripper, right_gripper, action_params: Dict): left_gripper_extent = action_params[ 'left_gripper_action_sample_extent'] right_gripper_extent = action_params[ 'right_gripper_action_sample_extent'] return FloatingRopeScenario.is_out_of_bounds(left_gripper, left_gripper_extent) \ or FloatingRopeScenario.is_out_of_bounds(right_gripper, right_gripper_extent) @staticmethod def is_out_of_bounds(p, extent): x, y, z = p x_min, x_max, y_min, y_max, z_min, z_max = extent return x < x_min or x > x_max \ or y < y_min or y > y_max \ or z < z_min or z > z_max @staticmethod def interpolate(start_state, end_state, step_size=0.05): left_gripper_start = np.array(start_state['left_gripper']) left_gripper_end = np.array(end_state['left_gripper']) right_gripper_start = np.array(start_state['right_gripper']) right_gripper_end = np.array(end_state['right_gripper']) left_gripper_delta = left_gripper_end - left_gripper_start right_gripper_delta = right_gripper_end - right_gripper_start left_gripper_steps = np.round( np.linalg.norm(left_gripper_delta) / step_size).astype(np.int64) right_gripper_steps = np.round( np.linalg.norm(right_gripper_delta) / step_size).astype(np.int64) steps = max(left_gripper_steps, right_gripper_steps) interpolated_actions = [] for t in np.linspace(step_size, 1, steps): left_gripper_i = left_gripper_start + left_gripper_delta * t right_gripper_i = right_gripper_start + right_gripper_delta * t action = { 'left_gripper_position': left_gripper_i, 'right_gripper_position': right_gripper_i, } interpolated_actions.append(action) return interpolated_actions @staticmethod def robot_name(): return "rope_3d" def randomize_environment(self, env_rng, params: Dict): pass @staticmethod def put_state_robot_frame(state: Dict): rope = state[rope_key_name] rope_points_shape = rope.shape[:-1].as_list() + [-1, 3] rope_points = tf.reshape(rope, rope_points_shape) # This assumes robot is at 0 0 0 robot_position = tf.constant([[0, 0, 0]], tf.float32) left_gripper_robot = state['left_gripper'] right_gripper_robot = state['right_gripper'] rope_points_robot = rope_points - tf.expand_dims(robot_position, axis=-2) rope_robot = tf.reshape(rope_points_robot, rope.shape) return { 'left_gripper': left_gripper_robot, 'right_gripper': right_gripper_robot, rope_key_name: rope_robot, } @staticmethod def put_state_local_frame(state: Dict): rope = state[rope_key_name] rope_points_shape = rope.shape[:-1].as_list() + [-1, 3] rope_points = tf.reshape(rope, rope_points_shape) center = tf.reduce_mean(rope_points, axis=-2) left_gripper_local = state['left_gripper'] - center right_gripper_local = state['right_gripper'] - center rope_points_local = rope_points - tf.expand_dims(center, axis=-2) rope_local = tf.reshape(rope_points_local, rope.shape) return { 'left_gripper': left_gripper_local, 'right_gripper': right_gripper_local, rope_key_name: rope_local, } @staticmethod def local_environment_center_differentiable(state): rope_vector = state[rope_key_name] rope_points = tf.reshape(rope_vector, [rope_vector.shape[0], -1, 3]) center = tf.reduce_mean(rope_points, axis=1) return center @staticmethod def apply_local_action_at_state(state, local_action): return { 'left_gripper_position': state['left_gripper'] + local_action['left_gripper_delta'], 'right_gripper_position': state['right_gripper'] + local_action['right_gripper_delta'] } @staticmethod def add_noise(action: Dict, noise_rng: np.random.RandomState): left_gripper_noise = noise_rng.normal(scale=0.01, size=[3]) right_gripper_noise = noise_rng.normal(scale=0.01, size=[3]) return { 'left_gripper_position': action['left_gripper_position'] + left_gripper_noise, 'right_gripper_position': action['right_gripper_position'] + right_gripper_noise } @staticmethod def integrate_dynamics(s_t: Dict, delta_s_t: Dict): return {k: s_t[k] + delta_s_t[k] for k in s_t.keys()} @staticmethod def put_action_local_frame(state: Dict, action: Dict): target_left_gripper_position = action['left_gripper_position'] target_right_gripper_position = action['right_gripper_position'] n_action = target_left_gripper_position.shape[1] current_left_gripper_point = state['left_gripper'] current_right_gripper_point = state['right_gripper'] left_gripper_delta = target_left_gripper_position - current_left_gripper_point[:, : n_action] right_gripper_delta = target_right_gripper_position - current_right_gripper_point[:, : n_action] return { 'left_gripper_delta': left_gripper_delta, 'right_gripper_delta': right_gripper_delta, } @staticmethod def state_to_gripper_position(state: Dict): gripper_position1 = np.reshape(state['left_gripper'], [3]) gripper_position2 = np.reshape(state['right_gripper'], [3]) return gripper_position1, gripper_position2 def get_cdcpd_state(self): cdcpd_msg: PointCloud2 = self.cdcpd_listener.get() # transform into robot-frame transform = self.tf.get_transform_msg("robot_root", "kinect2_rgb_optical_frame") cdcpd_points_robot_frame = tf2_sensor_msgs.do_transform_cloud( cdcpd_msg, transform) cdcpd_points_array = ros_numpy.numpify(cdcpd_points_robot_frame) x = cdcpd_points_array['x'] y = cdcpd_points_array['y'] z = cdcpd_points_array['z'] points = np.stack([x, y, z], axis=-1) cdcpd_vector = points.flatten() return cdcpd_vector def get_rope_state(self): rope_res = self.get_rope_srv(GetRopeStateRequest()) rope_state_vector = [] for p in rope_res.positions: rope_state_vector.append(p.x) rope_state_vector.append(p.y) rope_state_vector.append(p.z) rope_velocity_vector = [] for v in rope_res.velocities: rope_velocity_vector.append(v.x) rope_velocity_vector.append(v.y) rope_velocity_vector.append(v.z) return rope_state_vector def get_rope_point_positions(self): # NOTE: consider getting rid of this message type/service just use rope state [0] and rope state [-1] # although that looses semantic meaning and means hard-coding indices a lot... left_res: GetPosition3DResponse = self.pos3d.get( scoped_link_name=gz_scope(self.ROPE_NAMESPACE, 'left_gripper')) left_rope_point_position = ros_numpy.numpify(left_res.pos) right_res: GetPosition3DResponse = self.pos3d.get( scoped_link_name=gz_scope(self.ROPE_NAMESPACE, 'right_gripper')) right_rope_point_position = ros_numpy.numpify(right_res.pos) return left_rope_point_position, right_rope_point_position def get_state(self): color_depth_cropped = self.get_rgbd() rope_state_vector = self.get_rope_state() cdcpd_vector = self.get_cdcpd_state() left_rope_point_position, right_rope_point_position = self.get_rope_point_positions( ) return { 'left_gripper': left_rope_point_position, 'right_gripper': right_rope_point_position, 'gt_rope': np.array(rope_state_vector, np.float32), rope_key_name: np.array(cdcpd_vector, np.float32), 'rgbd': color_depth_cropped, } def get_rgbd(self): color_msg: Image = self.color_image_listener.get() depth_msg = self.depth_image_listener.get() depth = np.expand_dims(ros_numpy.numpify(depth_msg), axis=-1) bgr = ros_numpy.numpify(color_msg) rgb = np.flip(bgr, axis=2) # NaN Depths means out of range, so clip to the max range depth = np.clip(np.nan_to_num(depth, nan=KINECT_MAX_DEPTH), 0, KINECT_MAX_DEPTH) rgbd = np.concatenate([rgb, depth], axis=2) box = tf.convert_to_tensor([ self.crop_region['min_y'] / rgb.shape[0], self.crop_region['min_x'] / rgb.shape[1], self.crop_region['max_y'] / rgb.shape[0], self.crop_region['max_x'] / rgb.shape[1] ], dtype=tf.float32) # this operates on a batch rgbd_cropped = tf.image.crop_and_resize( image=tf.expand_dims(rgbd, axis=0), boxes=tf.expand_dims(box, axis=0), box_indices=[0], crop_size=[self.IMAGE_H, self.IMAGE_W]) rgbd_cropped = remove_batch(rgbd_cropped) def _debug_show_image(_rgb_depth_cropped): import matplotlib.pyplot as plt plt.imshow(tf.cast(_rgb_depth_cropped[:, :, :3], tf.int32)) plt.show() # BEGIN DEBUG # _debug_show_image(rgbd_cropped) # END DEBUG return rgbd_cropped.numpy() def observations_description(self) -> Dict: return { 'left_gripper': 3, 'right_gripper': 3, 'rgbd': [self.IMAGE_H, self.IMAGE_W, 4], } @staticmethod def states_description() -> Dict: return {} @staticmethod def observation_features_description() -> Dict: return { rope_key_name: FloatingRopeScenario.n_links * 3, 'cdcpd': FloatingRopeScenario.n_links * 3, } @staticmethod def actions_description() -> Dict: # should match the keys of the dict return from action_to_dataset_action return { 'left_gripper_position': 3, 'right_gripper_position': 3, } @staticmethod def state_to_points_for_cc(state: Dict): return state[rope_key_name].reshape(-1, 3) def __repr__(self): return "DualFloatingGripperRope" def simple_name(self): return "dual_floating" @staticmethod def sample_gripper_goal(environment: Dict, rng: np.random.RandomState, planner_params: Dict): env_inflated = inflate_tf_3d( env=environment['env'], radius_m=planner_params['goal_params']['threshold'], res=environment['res']) goal_extent = planner_params['goal_params']['extent'] while True: extent = np.array(goal_extent).reshape(3, 2) left_gripper = rng.uniform(extent[:, 0], extent[:, 1]) right_gripper = rng.uniform(extent[:, 0], extent[:, 1]) goal = { 'left_gripper': left_gripper, 'right_gripper': right_gripper, } row1, col1, channel1 = grid_utils.point_to_idx_3d_in_env( left_gripper[0], left_gripper[1], left_gripper[2], environment) row2, col2, channel2 = grid_utils.point_to_idx_3d_in_env( right_gripper[0], right_gripper[1], right_gripper[2], environment) collision1 = env_inflated[row1, col1, channel1] > 0.5 collision2 = env_inflated[row2, col2, channel2] > 0.5 if not collision1 and not collision2: return goal def sample_goal(self, environment: Dict, rng: np.random.RandomState, planner_params: Dict): goal_type = planner_params['goal_params']['goal_type'] if goal_type == 'midpoint': return self.sample_midpoint_goal(environment, rng, planner_params) else: raise NotImplementedError( planner_params['goal_params']['goal_type']) @staticmethod def distance_to_gripper_goal(state: Dict, goal: Dict): left_gripper = state['left_gripper'] right_gripper = state['right_gripper'] distance1 = np.linalg.norm(goal['left_gripper'] - left_gripper) distance2 = np.linalg.norm(goal['right_gripper'] - right_gripper) return max(distance1, distance2) def sample_midpoint_goal(self, environment: Dict, rng: np.random.RandomState, planner_params: Dict): goal_extent = planner_params['goal_params']['extent'] if environment == {}: rospy.loginfo("Assuming no obstacles in the environment") extent = np.array(goal_extent).reshape(3, 2) p = rng.uniform(extent[:, 0], extent[:, 1]) goal = {'midpoint': p} return goal env_inflated = inflate_tf_3d( env=environment['env'], radius_m=planner_params['goal_params']['threshold'], res=environment['res']) # DEBUG visualize the inflated env # from copy import deepcopy # environment_ = deepcopy(environment) # environment_['env'] = env_inflated # self.plot_environment_rviz(environment_) # END DEBUG while True: extent = np.array(goal_extent).reshape(3, 2) p = rng.uniform(extent[:, 0], extent[:, 1]) goal = {'midpoint': p} row, col, channel = grid_utils.point_to_idx_3d_in_env( p[0], p[1], p[2], environment) collision = env_inflated[row, col, channel] > 0.5 if not collision: return goal @staticmethod def distance_grippers_and_any_point_goal(state: Dict, goal: Dict): rope_points = np.reshape(state[rope_key_name], [-1, 3]) # well ok not _any_ node, but ones near the middle n_from_ends = 5 distances = np.linalg.norm(np.expand_dims(goal['point'], axis=0) - rope_points, axis=1)[n_from_ends:-n_from_ends] rope_distance = np.min(distances) left_gripper = state['left_gripper'] right_gripper = state['right_gripper'] distance1 = np.linalg.norm(goal['left_gripper'] - left_gripper) distance2 = np.linalg.norm(goal['right_gripper'] - right_gripper) return max(max(distance1, distance2), rope_distance) @staticmethod def distance_to_any_point_goal(state: Dict, goal: Dict): rope_points = np.reshape(state[rope_key_name], [-1, 3]) # well ok not _any_ node, but ones near the middle n_from_ends = 7 distances = np.linalg.norm(np.expand_dims(goal['point'], axis=0) - rope_points, axis=1)[n_from_ends:-n_from_ends] min_distance = np.min(distances) return min_distance @staticmethod def distance_to_midpoint_goal(state: Dict, goal: Dict): rope_points = np.reshape(state[rope_key_name], [-1, 3]) rope_midpoint = rope_points[int(FloatingRopeScenario.n_links / 2)] distance = np.linalg.norm(goal['midpoint'] - rope_midpoint) return distance def classifier_distance(self, s1: Dict, s2: Dict): model_error = np.linalg.norm(s1[rope_key_name] - s2[rope_key_name], axis=-1) # labeling_states = s1['rope'] # labeling_predicted_states = s2['rope'] # points_shape = labeling_states.shape.as_list()[:2] + [-1, 3] # labeling_points = tf.reshape(labeling_states, points_shape) # labeling_predicted_points = tf.reshape(labeling_predicted_states, points_shape) # model_error = tf.reduce_mean(tf.linalg.norm(labeling_points - labeling_predicted_points, axis=-1), axis=-1) return model_error def compute_label(self, actual: Dict, predicted: Dict, labeling_params: Dict): # NOTE: this should be using the same distance metric as the planning, which should also be the same as the labeling # done when making the classifier dataset actual_rope = np.array(actual["rope"]) predicted_rope = np.array(predicted["rope"]) model_error = np.linalg.norm(actual_rope - predicted_rope) threshold = labeling_params['threshold'] is_close = model_error < threshold return is_close def distance_to_goal(self, state, goal): if 'type' not in goal or goal['type'] == 'midpoint': return self.distance_to_midpoint_goal(state, goal) elif goal['type'] == 'any_point': return self.distance_to_any_point_goal(state, goal) elif goal['type'] == 'grippers': return self.distance_to_gripper_goal(state, goal) elif goal['type'] == 'grippers_and_point': return self.distance_grippers_and_any_point_goal(state, goal) else: raise NotImplementedError() def plot_goal_rviz(self, goal: Dict, goal_threshold: float, actually_at_goal: Optional[bool] = None): if actually_at_goal: r = 0.4 g = 0.8 b = 0.4 a = 0.6 else: r = 0.5 g = 0.3 b = 0.8 a = 0.6 goal_marker_msg = MarkerArray() if 'midpoint' in goal: midpoint_marker = Marker() midpoint_marker.scale.x = goal_threshold * 2 midpoint_marker.scale.y = goal_threshold * 2 midpoint_marker.scale.z = goal_threshold * 2 midpoint_marker.action = Marker.ADD midpoint_marker.type = Marker.SPHERE midpoint_marker.header.frame_id = "world" midpoint_marker.header.stamp = rospy.Time.now() midpoint_marker.ns = 'goal' midpoint_marker.id = 0 midpoint_marker.color.r = r midpoint_marker.color.g = g midpoint_marker.color.b = b midpoint_marker.color.a = a midpoint_marker.pose.position.x = goal['midpoint'][0] midpoint_marker.pose.position.y = goal['midpoint'][1] midpoint_marker.pose.position.z = goal['midpoint'][2] midpoint_marker.pose.orientation.w = 1 goal_marker_msg.markers.append(midpoint_marker) if 'point' in goal: point_marker = Marker() point_marker.scale.x = goal_threshold * 2 point_marker.scale.y = goal_threshold * 2 point_marker.scale.z = goal_threshold * 2 point_marker.action = Marker.ADD point_marker.type = Marker.SPHERE point_marker.header.frame_id = "world" point_marker.header.stamp = rospy.Time.now() point_marker.ns = 'goal' point_marker.id = 0 point_marker.color.r = r point_marker.color.g = g point_marker.color.b = b point_marker.color.a = a point_marker.pose.position.x = goal['point'][0] point_marker.pose.position.y = goal['point'][1] point_marker.pose.position.z = goal['point'][2] point_marker.pose.orientation.w = 1 goal_marker_msg.markers.append(point_marker) if 'left_gripper' in goal: left_gripper_marker = Marker() left_gripper_marker.scale.x = goal_threshold * 2 left_gripper_marker.scale.y = goal_threshold * 2 left_gripper_marker.scale.z = goal_threshold * 2 left_gripper_marker.action = Marker.ADD left_gripper_marker.type = Marker.SPHERE left_gripper_marker.header.frame_id = "world" left_gripper_marker.header.stamp = rospy.Time.now() left_gripper_marker.ns = 'goal' left_gripper_marker.id = 1 left_gripper_marker.color.r = r left_gripper_marker.color.g = g left_gripper_marker.color.b = b left_gripper_marker.color.a = a left_gripper_marker.pose.position.x = goal['left_gripper'][0] left_gripper_marker.pose.position.y = goal['left_gripper'][1] left_gripper_marker.pose.position.z = goal['left_gripper'][2] left_gripper_marker.pose.orientation.w = 1 goal_marker_msg.markers.append(left_gripper_marker) if 'right_gripper' in goal: right_gripper_marker = Marker() right_gripper_marker.scale.x = goal_threshold * 2 right_gripper_marker.scale.y = goal_threshold * 2 right_gripper_marker.scale.z = goal_threshold * 2 right_gripper_marker.action = Marker.ADD right_gripper_marker.type = Marker.SPHERE right_gripper_marker.header.frame_id = "world" right_gripper_marker.header.stamp = rospy.Time.now() right_gripper_marker.ns = 'goal' right_gripper_marker.id = 2 right_gripper_marker.color.r = r right_gripper_marker.color.g = g right_gripper_marker.color.b = b right_gripper_marker.color.a = a right_gripper_marker.pose.position.x = goal['right_gripper'][0] right_gripper_marker.pose.position.y = goal['right_gripper'][1] right_gripper_marker.pose.position.z = goal['right_gripper'][2] right_gripper_marker.pose.orientation.w = 1 goal_marker_msg.markers.append(right_gripper_marker) self.state_viz_pub.publish(goal_marker_msg) def plot_goal_boxes(self, goal: Dict, goal_threshold: float, actually_at_goal: Optional[bool] = None): if actually_at_goal: r = 0.4 g = 0.8 b = 0.4 a = 0.6 else: r = 0.5 g = 0.3 b = 0.8 a = 0.6 goal_marker_msg = MarkerArray() if 'point_box' in goal: point_marker = make_box_marker_from_extents(goal['point_box']) point_marker.header.frame_id = "world" point_marker.header.stamp = rospy.Time.now() point_marker.ns = 'goal' point_marker.id = 0 point_marker.color.r = r point_marker.color.g = g point_marker.color.b = b point_marker.color.a = a goal_marker_msg.markers.append(point_marker) if 'left_gripper_box' in goal: left_gripper_marker = make_box_marker_from_extents( goal['left_gripper_box']) left_gripper_marker.header.frame_id = "world" left_gripper_marker.header.stamp = rospy.Time.now() left_gripper_marker.ns = 'goal' left_gripper_marker.id = 1 left_gripper_marker.color.r = r left_gripper_marker.color.g = g left_gripper_marker.color.b = b left_gripper_marker.color.a = a goal_marker_msg.markers.append(left_gripper_marker) if 'right_gripper_box' in goal: right_gripper_marker = make_box_marker_from_extents( goal['right_gripper_box']) right_gripper_marker.header.frame_id = "world" right_gripper_marker.header.stamp = rospy.Time.now() right_gripper_marker.ns = 'goal' right_gripper_marker.id = 2 right_gripper_marker.color.r = r right_gripper_marker.color.g = g right_gripper_marker.color.b = b right_gripper_marker.color.a = a goal_marker_msg.markers.append(right_gripper_marker) self.state_viz_pub.publish(goal_marker_msg) @staticmethod def dynamics_loss_function(dataset_element, predictions): return dynamics_loss_function(dataset_element, predictions) @staticmethod def dynamics_metrics_function(dataset_element, predictions): return dynamics_points_metrics_function(dataset_element, predictions) def plot_tree_action(self, state: Dict, action: Dict, **kwargs): r = kwargs.pop("r", 0.2) g = kwargs.pop("g", 0.2) b = kwargs.pop("b", 0.8) a = kwargs.pop("a", 1.0) ig = marker_index_generator(self.tree_action_idx) idx1 = next(ig) idx2 = next(ig) self.plot_action_rviz(state, action, label='tree', color=[r, g, b, a], idx1=idx1, idx2=idx2, **kwargs) self.tree_action_idx += 1 def plot_state_rviz(self, state: Dict, label: str, **kwargs): r, g, b, a = colors.to_rgba(kwargs.get("color", "r")) idx = kwargs.get("idx", 0) msg = MarkerArray() ig = marker_index_generator(idx) if 'gt_rope' in state: rope_points = np.reshape(state['gt_rope'], [-1, 3]) markers = make_rope_marker(rope_points, 'world', label + "_gt_rope", next(ig), r, g, b, a) msg.markers.extend(markers) if 'rope' in state: rope_points = np.reshape(state['rope'], [-1, 3]) markers = make_rope_marker(rope_points, 'world', label + "_rope", next(ig), r, g, b, a) msg.markers.extend(markers) if add_predicted('rope') in state: rope_points = np.reshape(state[add_predicted('rope')], [-1, 3]) markers = make_rope_marker(rope_points, 'world', label + "_pred_rope", next(ig), r, g, b, a, Marker.CUBE_LIST) msg.markers.extend(markers) if 'left_gripper' in state: r = 0.2 g = 0.2 b = 0.8 left_gripper_sphere = make_gripper_marker(state['left_gripper'], next(ig), r, g, b, a, label + '_l', Marker.SPHERE) msg.markers.append(left_gripper_sphere) if 'right_gripper' in state: r = 0.8 g = 0.8 b = 0.2 right_gripper_sphere = make_gripper_marker(state['right_gripper'], next(ig), r, g, b, a, label + "_r", Marker.SPHERE) msg.markers.append(right_gripper_sphere) if add_predicted('left_gripper') in state: r = 0.2 g = 0.2 b = 0.8 lgpp = state[add_predicted('left_gripper')] left_gripper_sphere = make_gripper_marker(lgpp, next(ig), r, g, b, a, label + "_lp", Marker.CUBE) msg.markers.append(left_gripper_sphere) if add_predicted('right_gripper') in state: r = 0.8 g = 0.8 b = 0.2 rgpp = state[add_predicted('right_gripper')] right_gripper_sphere = make_gripper_marker(rgpp, next(ig), r, g, b, a, label + "_rp", Marker.CUBE) msg.markers.append(right_gripper_sphere) s = kwargs.get("scale", 1.0) msg = scale_marker_array(msg, s) self.state_viz_pub.publish(msg) if in_maybe_predicted('rgbd', state): publish_color_image(self.state_color_viz_pub, state['rgbd'][:, :, :3]) publish_depth_image(self.state_depth_viz_pub, state['rgbd'][:, :, 3]) if add_predicted('stdev') in state: stdev_t = state[add_predicted('stdev')][0] self.plot_stdev(stdev_t) if 'error' in state: error_msg = Float32() error_t = state['error'] error_msg.data = error_t self.error_pub.publish(error_msg) def plot_action_rviz(self, state: Dict, action: Dict, label: str = 'action', **kwargs): state_action = {} state_action.update(state) state_action.update(action) self.plot_action_rviz_internal(state_action, label=label, **kwargs) def plot_action_rviz_internal(self, data: Dict, label: str, **kwargs): r, g, b, a = colors.to_rgba(kwargs.get("color", "b")) s1 = np.reshape(get_maybe_predicted(data, 'left_gripper'), [3]) s2 = np.reshape(get_maybe_predicted(data, 'right_gripper'), [3]) a1 = np.reshape(get_maybe_predicted(data, 'left_gripper_position'), [3]) a2 = np.reshape(get_maybe_predicted(data, 'right_gripper_position'), [3]) idx = kwargs.pop("idx", None) ig = marker_index_generator(idx) if idx is not None: idx1 = next(ig) idx2 = next(ig) else: idx1 = kwargs.pop("idx1", 0) idx2 = kwargs.pop("idx2", 1) msg = MarkerArray() msg.markers.append( rviz_arrow(s1, a1, r, g, b, a, idx=idx1, label=label, **kwargs)) msg.markers.append( rviz_arrow(s2, a2, r, g, b, a, idx=idx2, label=label, **kwargs)) self.action_viz_pub.publish(msg) def register_fake_grasping(self): register_left_req = RegisterPosition3DControllerRequest() register_left_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE, "left_gripper") register_left_req.controller_type = "kinematic" self.pos3d.register(register_left_req) register_right_req = RegisterPosition3DControllerRequest() register_right_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE, "right_gripper") register_right_req.controller_type = "kinematic" self.pos3d.register(register_right_req) def make_rope_endpoints_follow_gripper(self): left_follow_req = Position3DFollowRequest() left_follow_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE, "left_gripper") left_follow_req.frame_id = "left_tool" self.pos3d.follow(left_follow_req) right_follow_req = Position3DFollowRequest() right_follow_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE, "right_gripper") right_follow_req.frame_id = "right_tool" self.pos3d.follow(right_follow_req)
def get_camera_params(camera_name: str): camera_params_topic_name = ns_join(ns_join(camera_name, 'qhd'), "camera_info") camera_params_listener = Listener(camera_params_topic_name, CameraInfo) camera_params: CameraInfo = camera_params_listener.get() return camera_params
class BaseVictor(DualArmRobot): def __init__(self, robot_namespace: str): DualArmRobot.__init__(self, robot_namespace=robot_namespace) self.left_arm_command_pub = rospy.Publisher( self.ns("left_arm/motion_command"), MotionCommand, queue_size=10) self.right_arm_command_pub = rospy.Publisher( self.ns("right_arm/motion_command"), MotionCommand, queue_size=10) self.left_gripper_command_pub = rospy.Publisher( self.ns("left_arm/gripper_command"), Robotiq3FingerCommand, queue_size=10) self.right_gripper_command_pub = rospy.Publisher( self.ns("right_arm/gripper_command"), Robotiq3FingerCommand, queue_size=10) self.left_set_control_mode_srv = rospy.ServiceProxy( self.ns("left_arm/set_control_mode_service"), SetControlMode) self.right_set_control_mode_srv = rospy.ServiceProxy( self.ns("right_arm/set_control_mode_service"), SetControlMode) self.left_get_control_mode_srv = rospy.ServiceProxy( self.ns("left_arm/get_control_mode_service"), GetControlMode) self.right_get_control_mode_srv = rospy.ServiceProxy( self.ns("right_arm/get_control_mode_service"), GetControlMode) self.left_arm_status_listener = Listener( self.ns("left_arm/motion_status"), MotionStatus) self.right_arm_status_listener = Listener( self.ns("right_arm/motion_status"), MotionStatus) self.left_gripper_status_listener = Listener( self.ns("left_arm/gripper_status"), Robotiq3FingerStatus) self.right_gripper_status_listener = Listener( self.ns("right_arm/gripper_status"), Robotiq3FingerStatus) self.waypoint_state_pub = rospy.Publisher( self.ns("waypoint_robot_state"), DisplayRobotState, queue_size=10) def send_joint_command( self, joint_names: List[str], trajectory_point: JointTrajectoryPoint) -> Tuple[bool, str]: # TODO: in victor's impedance mode, we want to modify the setpoint so that there is a limit # on the force we will apply positions_by_interface, abort, msg = delegate_positions_to_arms( trajectory_point.positions, joint_names) left_arm_positions = positions_by_interface['left_arm'] right_arm_positions = positions_by_interface['right_arm'] left_gripper_positions = positions_by_interface['left_gripper'] right_gripper_positions = positions_by_interface['right_gripper'] if abort: return True, msg # Get the current control mode control_mode = self.get_control_modes() left_arm_control_mode = control_mode['left'] right_arm_control_mode = control_mode['right'] self.send_arm_command(self.left_arm_command_pub, left_arm_control_mode, left_arm_positions) self.send_arm_command(self.right_arm_command_pub, right_arm_control_mode, right_arm_positions) self.send_gripper_command(self.left_gripper_command_pub, left_gripper_positions) self.send_gripper_command(self.right_gripper_command_pub, right_gripper_positions) return False, "" def send_arm_command(self, command_pub: rospy.Publisher, right_arm_control_mode: ControlMode, positions): if positions is not None: # FIXME: what if we allow the BaseRobot class to use moveit, but just don't have it require that # any actions are running? # NOTE: why are these values not checked by the lower-level code? the Java code knows what the joint limits # are so why does it not enforce them? # TODO: use enforce bounds? https://github.com/ros-planning/moveit/pull/2356 limit_enforced_positions = [] for i, joint_name in enumerate(right_arm_joints): joint: moveit_commander.RobotCommander.Joint = self.robot_commander.get_joint( joint_name) limit_enforced_position = np.clip(positions[i], joint.min_bound() + 1e-2, joint.max_bound() - 1e-2) limit_enforced_positions.append(limit_enforced_position) cmd = MotionCommand() cmd.header.stamp = rospy.Time.now() cmd.joint_position = list_to_jvq(limit_enforced_positions) cmd.control_mode = right_arm_control_mode command_pub.publish(cmd) def get_gripper_statuses(self): return { 'left': self.get_left_gripper_status(), 'right': self.get_right_gripper_status() } def get_right_gripper_status(self): right_status: Robotiq3FingerStatus = self.right_gripper_status_listener.get( ) return right_status def get_left_gripper_status(self): left_status: Robotiq3FingerStatus = self.left_gripper_status_listener.get( ) return left_status def is_left_gripper_closed(self): status = self.get_left_gripper_status() return self.is_gripper_closed(status) def is_right_gripper_closed(self): status = self.get_right_gripper_status() return self.is_gripper_closed(status) def is_gripper_closed(self, status: Robotiq3FingerStatus): # [0.5, 0.4, 0.4, 0.8] finger_a_closed = status.finger_a_status.position > 0.5 - 1e-2 finger_b_closed = status.finger_b_status.position > 0.5 - 1e-2 finger_c_closed = status.finger_c_status.position > 0.5 - 1e-2 return finger_a_closed and finger_b_closed and finger_c_closed def get_arms_statuses(self): return { 'left': self.get_left_arm_status(), 'right': self.get_right_arm_status() } def get_right_arm_status(self): right_status: MotionStatus = self.right_arm_status_listener.get() return right_status def get_left_arm_status(self): left_status: MotionStatus = self.left_arm_status_listener.get() return left_status def get_control_modes(self): return { 'left': self.get_left_arm_control_mode(), 'right': self.get_right_arm_control_mode() } def set_control_mode(self, control_mode: ControlMode, **kwargs): left_res = self.set_left_arm_control_mode(control_mode, **kwargs) right_res = self.set_right_arm_control_mode(control_mode, **kwargs) return left_res, right_res def get_left_arm_control_mode(self): left_control_mode_res: GetControlModeResponse = self.left_get_control_mode_srv( GetControlModeRequest()) return left_control_mode_res.active_control_mode.control_mode def get_right_arm_control_mode(self): right_control_mode_res: GetControlModeResponse = self.right_get_control_mode_srv( GetControlModeRequest()) return right_control_mode_res.active_control_mode.control_mode def set_right_arm_control_mode(self, control_mode: ControlMode, **kwargs): new_control_mode = get_control_mode_params(control_mode, **kwargs) res: SetControlModeResponse = self.right_set_control_mode_srv( new_control_mode) if not res.success: rospy.logerr("Failed to switch right arm to control mode: " + str(control_mode)) rospy.logerr(res.message) return res def set_left_arm_control_mode(self, control_mode: ControlMode, **kwargs): new_control_mode = get_control_mode_params(control_mode, **kwargs) res: SetControlModeResponse = self.left_set_control_mode_srv( new_control_mode) if not res.success: rospy.logerr("Failed to switch left arm to control mode: " + str(control_mode)) rospy.logerr(res.message) return res def send_cartesian_command(self, poses: Dict): """ absolute """ self.send_left_arm_cartesian_command(poses['left']) self.send_right_arm_cartesian_command(poses['right']) def send_left_arm_cartesian_command(self, pose_stamped): pose_stamped = convert_to_pose_msg(pose_stamped) pose_stamped.pose.orientation = normalize_quaternion( pose_stamped.pose.orientation) left_arm_command = MotionCommand() left_arm_command.header.frame_id = 'victor_left_arm_world_frame_kuka' left_arm_command.cartesian_pose = pose_stamped.pose left_arm_command.control_mode = self.get_left_arm_control_mode() while self.left_arm_command_pub.get_num_connections() < 1: rospy.sleep(0.01) self.left_arm_command_pub.publish(left_arm_command) def send_right_arm_cartesian_command(self, pose_stamped): pose_stamped = convert_to_pose_msg(pose_stamped) pose_stamped.pose.orientation = normalize_quaternion( pose_stamped.pose.orientation) right_arm_command = MotionCommand() right_arm_command.header.frame_id = 'victor_right_arm_world_frame_kuka' right_arm_command.cartesian_pose = pose_stamped.pose right_arm_command.control_mode = self.get_right_arm_control_mode() while self.right_arm_command_pub.get_num_connections() < 1: rospy.sleep(0.01) self.right_arm_command_pub.publish(right_arm_command) def send_delta_cartesian_command(self, delta_positions): delta_positions = convert_to_positions(delta_positions) statuses = self.get_arms_statuses() current_left_pose = statuses['left'].measured_cartesian_pose desired_left_pose = current_left_pose desired_left_pose.position.x += delta_positions['left'].x desired_left_pose.position.y += delta_positions['left'].y desired_left_pose.position.z += delta_positions['left'].z current_right_pose = statuses['right'].measured_cartesian_pose desired_right_pose = current_right_pose desired_right_pose.position.x += delta_positions['right'].x desired_right_pose.position.y += delta_positions['right'].y desired_right_pose.position.z += delta_positions['right'].z poses = { 'left': desired_left_pose, 'right': desired_right_pose, } self.send_cartesian_command(poses) @staticmethod def send_gripper_command(command_pub: rospy.Publisher, positions): if positions is not None: cmd = default_gripper_command() cmd.header.stamp = rospy.Time.now() cmd.finger_a_command.position = positions[0] cmd.finger_b_command.position = positions[1] cmd.finger_c_command.position = positions[2] cmd.scissor_command.position = positions[3] command_pub.publish(cmd) def get_joint_positions(self, joint_names: Optional[List[str]] = None): """ :args joint_names an optional list of names if you want to have a specific order or a subset """ joint_state: JointState = self.joint_state_listener.get() if joint_names is None: return joint_state.position gripper_statuses = self.get_gripper_statuses() current_joint_positions = [] for name in joint_names: if name in joint_state.name: idx = joint_state.name.index(name) pos = joint_state.position[idx] current_joint_positions.append(pos) elif name == 'left_finger_a': current_joint_positions.append( gripper_statuses['left'].finger_a_status.position) elif name == 'left_finger_b': current_joint_positions.append( gripper_statuses['left'].finger_b_status.position) elif name == 'left_finger_c': current_joint_positions.append( gripper_statuses['left'].finger_c_status.position) elif name == 'left_scissor': current_joint_positions.append( gripper_statuses['left'].scissor_status.position) elif name == 'right_finger_a': current_joint_positions.append( gripper_statuses['right'].finger_a_status.position) elif name == 'right_finger_b': current_joint_positions.append( gripper_statuses['right'].finger_b_status.position) elif name == 'right_finger_c': current_joint_positions.append( gripper_statuses['right'].finger_c_status.position) elif name == 'right_scissor': current_joint_positions.append( gripper_statuses['right'].scissor_status.position) else: raise ValueError(f"Could not get joint {name}") # try looking at the status messages return current_joint_positions def get_right_gripper_command_pub(self): return self.right_gripper_command_pub def get_left_gripper_command_pub(self): return self.left_gripper_command_pub def get_open_gripper_msg(self): cmd = default_robotiq_command() cmd.finger_a_command.position = 0.25 cmd.finger_b_command.position = 0.25 cmd.finger_c_command.position = 0.25 cmd.scissor_command.position = 0.8 return cmd def get_close_gripper_msg(self): cmd = default_robotiq_command() cmd.finger_a_command.position = 0.65 cmd.finger_b_command.position = 0.65 cmd.finger_c_command.position = 0.5 cmd.scissor_command.position = 0.8 return cmd
def __init__(self, planner: MyPlanner, trials: List[int], verbose: int, planner_params: Dict, service_provider: BaseServices, no_execution: bool, test_scenes_dir: Optional[pathlib.Path] = None, save_test_scenes_dir: Optional[pathlib.Path] = None, ): self.planner = planner self.scenario = self.planner.scenario self.scenario.on_before_get_state_or_execute_action() self.trials = trials self.planner_params = planner_params self.verbose = verbose self.service_provider = service_provider self.no_execution = no_execution self.env_rng = np.random.RandomState(0) self.goal_rng = np.random.RandomState(0) self.recovery_rng = np.random.RandomState(0) self.test_scenes_dir = test_scenes_dir self.save_test_scenes_dir = save_test_scenes_dir if self.planner_params['recovery']['use_recovery']: recovery_model_dir = pathlib.Path(self.planner_params['recovery']['recovery_model_dir']) self.recovery_policy = recovery_policy_utils.load_generic_model(model_dir=recovery_model_dir, scenario=self.scenario, rng=self.recovery_rng) else: self.recovery_policy = None self.n_failures = 0 # for saving snapshots of the world self.link_states_listener = Listener("gazebo/link_states", LinkStates) # Debugging if self.verbose >= 2: self.goal_bbox_pub = rospy.Publisher('goal_bbox', BoundingBox, queue_size=10, latch=True) bbox_msg = extent_to_bbox(planner_params['goal_params']['extent']) bbox_msg.header.frame_id = 'world' self.goal_bbox_pub.publish(bbox_msg) goal_params = self.planner_params['goal_params'] if goal_params['type'] == 'fixed': self.goal_generator = lambda e: numpify(goal_params['goal_fixed']) elif goal_params['type'] == 'random': self.goal_generator = lambda e: self.scenario.sample_goal(environment=e, rng=self.goal_rng, planner_params=self.planner_params) elif goal_params['type'] == 'dataset': dataset = DynamicsDatasetLoader([pathlib.Path(goal_params['goals_dataset'])]) tf_dataset = dataset.get_datasets(mode='val') goal_dataset_iterator = iter(tf_dataset) def _gen(e): example = next(goal_dataset_iterator) example_t = dataset.index_time_batched(example_batched=add_batch(example), t=1) goal = remove_batch(example_t) return goal self.goal_generator = _gen else: raise NotImplementedError(f"invalid goal param type {goal_params['type']}")
class PlanAndExecute: def __init__(self, planner: MyPlanner, trials: List[int], verbose: int, planner_params: Dict, service_provider: BaseServices, no_execution: bool, test_scenes_dir: Optional[pathlib.Path] = None, save_test_scenes_dir: Optional[pathlib.Path] = None, ): self.planner = planner self.scenario = self.planner.scenario self.scenario.on_before_get_state_or_execute_action() self.trials = trials self.planner_params = planner_params self.verbose = verbose self.service_provider = service_provider self.no_execution = no_execution self.env_rng = np.random.RandomState(0) self.goal_rng = np.random.RandomState(0) self.recovery_rng = np.random.RandomState(0) self.test_scenes_dir = test_scenes_dir self.save_test_scenes_dir = save_test_scenes_dir if self.planner_params['recovery']['use_recovery']: recovery_model_dir = pathlib.Path(self.planner_params['recovery']['recovery_model_dir']) self.recovery_policy = recovery_policy_utils.load_generic_model(model_dir=recovery_model_dir, scenario=self.scenario, rng=self.recovery_rng) else: self.recovery_policy = None self.n_failures = 0 # for saving snapshots of the world self.link_states_listener = Listener("gazebo/link_states", LinkStates) # Debugging if self.verbose >= 2: self.goal_bbox_pub = rospy.Publisher('goal_bbox', BoundingBox, queue_size=10, latch=True) bbox_msg = extent_to_bbox(planner_params['goal_params']['extent']) bbox_msg.header.frame_id = 'world' self.goal_bbox_pub.publish(bbox_msg) goal_params = self.planner_params['goal_params'] if goal_params['type'] == 'fixed': self.goal_generator = lambda e: numpify(goal_params['goal_fixed']) elif goal_params['type'] == 'random': self.goal_generator = lambda e: self.scenario.sample_goal(environment=e, rng=self.goal_rng, planner_params=self.planner_params) elif goal_params['type'] == 'dataset': dataset = DynamicsDatasetLoader([pathlib.Path(goal_params['goals_dataset'])]) tf_dataset = dataset.get_datasets(mode='val') goal_dataset_iterator = iter(tf_dataset) def _gen(e): example = next(goal_dataset_iterator) example_t = dataset.index_time_batched(example_batched=add_batch(example), t=1) goal = remove_batch(example_t) return goal self.goal_generator = _gen else: raise NotImplementedError(f"invalid goal param type {goal_params['type']}") def run(self): self.scenario.randomization_initialization() for trial_idx in self.trials: self.plan_and_execute(trial_idx) self.on_complete() def plan(self, planning_query: PlanningQuery): ############ # Planning # ############ if self.verbose >= 1: (Fore.MAGENTA + "Planning to {}".format(planning_query.goal) + Fore.RESET) planning_result = self.planner.plan(planning_query=planning_query) rospy.loginfo(f"Planning time: {planning_result.time:5.3f}s, Status: {planning_result.status}") self.on_plan_complete(planning_query, planning_result) return planning_result def execute(self, planning_query: PlanningQuery, planning_result: PlanningResult): # execute the plan, collecting the states that actually occurred self.on_before_execute() if self.no_execution: state_t = self.scenario.get_state() actual_path = [state_t] else: if self.verbose >= 2 and not self.no_execution: rospy.loginfo(Fore.CYAN + "Executing Plan" + Fore.RESET) self.service_provider.play() actual_path = execute_actions(scenario=self.scenario, environment=planning_query.environment, start_state=planning_query.start, actions=planning_result.actions, plot=True) self.service_provider.pause() # post-execution callback execution_result = ExecutionResult(path=actual_path) return execution_result def execute_recovery_action(self, action: Dict): if self.no_execution: actual_path = [] else: before_state = self.scenario.get_state() self.service_provider.play() self.scenario.execute_action(action) self.service_provider.pause() after_state = self.scenario.get_state() actual_path = [before_state, after_state] execution_result = ExecutionResult(path=actual_path) return execution_result def get_environment(self): # get the environment, which here means anything which is assumed constant during planning return self.scenario.get_environment(self.planner.fwd_model.data_collection_params) def plan_and_execute(self, trial_idx: int): self.set_random_seeds_for_trial(trial_idx) self.save_or_restore_test_scene(trial_idx) self.on_start_trial(trial_idx) start_time = time.perf_counter() total_timeout = self.planner_params['total_timeout'] # Get the goal (default is to randomly sample one) goal = self.get_goal(self.get_environment()) attempt_idx = 0 steps_data = [] planning_queries = [] while True: # get start states start_state = self.scenario.get_state() # get the environment, which here means anything which is assumed constant during planning # This includes the occupancy map but can also include things like the initial state of the tether environment = self.get_environment() # Try to make the seeds reproducible, but it needs to change based on attempt idx or we would just keep # trying the same plans over and over seed = 100000 * trial_idx + attempt_idx planning_query = PlanningQuery(goal=goal, environment=environment, start=start_state, seed=seed) planning_queries.append(planning_query) planning_result = self.plan(planning_query) time_since_start = time.perf_counter() - start_time if planning_result.status == MyPlannerStatus.Failure: raise RuntimeError("planning failed -- is the start state out of bounds?") elif planning_result.status == MyPlannerStatus.NotProgressing: if self.recovery_policy is None: # Nothing else to do here, just give up end_state = self.scenario.get_state() trial_status = TrialStatus.NotProgressingNoRecovery trial_msg = f"Trial {trial_idx} Ended: not progressing, no recovery. {time_since_start:.3f}s" rospy.loginfo(Fore.BLUE + trial_msg + Fore.RESET) trial_data_dict = { 'planning_queries': planning_queries, 'total_time': time_since_start, 'trial_status': trial_status, 'trial_idx': trial_idx, 'end_state': end_state, 'goal': goal, 'steps': steps_data, } self.on_trial_complete(trial_data_dict, trial_idx) return else: recovery_action = self.recovery_policy(environment=planning_query.environment, state=planning_query.start) attempt_idx += 1 rospy.loginfo(f"Attempting recovery action {attempt_idx}") if self.verbose >= 3: rospy.loginfo("Chosen Recovery Action:") rospy.loginfo(recovery_action) execution_result = self.execute_recovery_action(recovery_action) # Extract planner data now before it goes out of scope (in C++) steps_data.append({ 'type': 'executed_recovery', 'planning_query': planning_query, 'planning_result': planning_result, 'recovery_action': recovery_action, 'execution_result': execution_result, 'time_since_start': time_since_start, }) else: execution_result = self.execute(planning_query, planning_result) steps_data.append({ 'type': 'executed_plan', 'planning_query': planning_query, 'planning_result': planning_result, 'execution_result': execution_result, 'time_since_start': time_since_start, }) self.on_execution_complete(planning_query, planning_result, execution_result) end_state = self.scenario.get_state() d = self.scenario.distance_to_goal(end_state, planning_query.goal) rospy.loginfo(f"distance to goal after execution is {d:.3f}") reached_goal = (d <= self.planner_params['goal_params']['threshold'] + 1e-6) if reached_goal or time_since_start > total_timeout: if reached_goal: trial_status = TrialStatus.Reached rospy.loginfo(Fore.BLUE + f"Trial {trial_idx} Ended: Goal reached!" + Fore.RESET) else: trial_status = TrialStatus.Timeout rospy.loginfo(Fore.BLUE + f"Trial {trial_idx} Ended: Timeout {time_since_start:.3f}s" + Fore.RESET) trial_data_dict = { 'planning_queries': planning_queries, 'total_time': time_since_start, 'trial_status': trial_status, 'trial_idx': trial_idx, 'goal': goal, 'steps': steps_data, 'end_state': end_state, } self.on_trial_complete(trial_data_dict, trial_idx) return def save_or_restore_test_scene(self, trial_idx): if self.test_scenes_dir is not None: # Gazebo specific bagfile_name = self.test_scenes_dir / f'scene_{trial_idx:04d}.bag' rospy.loginfo(Fore.GREEN + f"Restoring scene {bagfile_name}") self.scenario.before_restore() self.service_provider.pause() self.service_provider.restore_from_bag(bagfile_name) self.scenario.after_restore() self.service_provider.play() else: self.randomize_environment() rospy.loginfo(Fore.GREEN + f"Randomizing Environment") if self.save_test_scenes_dir is not None: # Gazebo specific links_states = self.link_states_listener.get() self.save_test_scenes_dir.mkdir(exist_ok=True, parents=True) bagfile_name = self.save_test_scenes_dir / f'scene_{trial_idx:04d}.bag' rospy.loginfo(f"Saving scene to {bagfile_name}") with rosbag.Bag(bagfile_name, 'w') as bag: bag.write('links_states', links_states) def set_random_seeds_for_trial(self, trial_idx): self.env_rng.seed(trial_idx) self.recovery_rng.seed(trial_idx) self.goal_rng.seed(trial_idx) # NOTE: ompl SetSeed can only be called once which is why we don't bother doing it here # FIXME: we should not be relying on this... np.random.seed(trial_idx) tf.random.set_seed(trial_idx) def on_trial_complete(self, trial_data, trial_idx: int): pass def get_goal(self, environment: Dict): return self.goal_generator(environment) def on_plan_complete(self, planning_query: PlanningQuery, planning_result: PlanningResult): # visualize the plan if self.verbose >= 1: self.scenario.animate_final_path(environment=planning_query.environment, planned_path=planning_result.path, actions=planning_result.actions) def on_before_execute(self): pass def on_start_trial(self, trial_idx: int): pass def on_execution_complete(self, planning_query: PlanningQuery, planning_result: PlanningResult, execution_result: ExecutionResult): pass def on_complete(self): pass def randomize_environment(self): self.scenario.randomize_environment(self.env_rng, self.planner_params)
if (right is not None) and (right.last_pos is not None): rospy.loginfo("Right: " + vec_to_rad_str(right.last_pos)) if __name__ == "__main__": rospy.init_node("manual_motion") control_mode_params = vu.get_joint_impedance_params(vu.Stiffness.MEDIUM) use_left_arm = rospy.get_param("~use_left_arm", True) use_right_arm = rospy.get_param("~use_right_arm", True) if use_left_arm: print("initializing left arm ...") sys.stdout.flush() l_cm = Listener("left_arm/control_mode_status", ControlModeParameters) cur_mode = l_cm.get(block_until_data=True) control_mode_params.joint_path_execution_params.joint_relative_velocity = cur_mode.joint_path_execution_params.joint_relative_velocity control_mode_params.joint_path_execution_params.joint_relative_acceleration = cur_mode.joint_path_execution_params.joint_relative_acceleration result = vu.send_new_control_mode("left_arm", control_mode_params) while not result.success: result = vu.send_new_control_mode("left_arm", control_mode_params) print("done") else: print("not using left arm") if use_right_arm: print("initializing right arm ...") r_cm = Listener("right_arm/control_mode_status", ControlModeParameters) cur_mode = r_cm.get(block_until_data=True)