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)