def slide_obstacles(self, env_rng, params: Dict):
# set random positions for all the objects
for object_name in params['objects']:
scoped_link_name = gz_scope(object_name, 'link_1')
get_res = self.pos3d.get(scoped_link_name=scoped_link_name)
pos_msg = get_res.pos
pos_msg.x = pos_msg.x + env_rng.uniform(-0.2, 0.2)
pos_msg.y = pos_msg.y + env_rng.uniform(-0.2, 0.2)
pos_msg.z = pos_msg.z + env_rng.uniform(-0.2, 0.2)
self.register_movable_object(scoped_link_name)
req = Position3DActionRequest(speed_mps=0.1,
scoped_link_name=scoped_link_name,
tolerance_m=0.01,
position=pos_msg)
self.pos3d.set(req)
wait_req = Position3DWaitRequest()
wait_req.timeout_s = 0.1
for object_name in params['objects']:
scoped_link_name = gz_scope(object_name, 'link_1')
wait_req.scoped_link_names.append(scoped_link_name)
self.pos3d.wait(wait_req)
for object_name in params['objects']:
scoped_link_name = gz_scope(object_name, 'link_1')
self.pos3d.enable(
Position3DEnableRequest(scoped_link_name=scoped_link_name,
enable=False))
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 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 move_rope_out_of_the_scene(self):
set_req = Position3DActionRequest()
set_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE,
"left_gripper")
set_req.position.x = 1.3
set_req.position.y = 0.3
set_req.position.z = 1.3
self.pos3d.set(set_req)
set_req = Position3DActionRequest()
set_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE,
"right_gripper")
set_req.position.x = 1.3
set_req.position.y = -0.3
set_req.position.z = 1.3
self.pos3d.set(set_req)
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)
class RopeDraggingScenario(Base3DScenario):
n_links = 10
ROPE_NAMESPACE = 'dragging_rope'
ROPE_LINK_NAME = gz_scope(ROPE_NAMESPACE, 'gripper1')
def __init__(self):
super().__init__()
self.service_provider = BaseServices()
self.pos3d = Position3D()
self.get_rope_srv = rospy.ServiceProxy(
ns_join(self.ROPE_NAMESPACE, "get_rope_state"), GetRopeState)
self.reset_sim_srv = rospy.ServiceProxy("/gazebo/reset_simulation",
Empty)
self.last_action = None
self.max_action_attempts = 1000
def __repr__(self):
return "rope_dragging_scenario"
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)
ig = marker_index_generator(idx)
msg = MarkerArray()
if rope_key_name in state:
rope_points = np.reshape(state[rope_key_name], [-1, 3])
rope_mrkrs = make_rope_marker(rope_points,
'world',
label + "_gt_" + rope_key_name,
next(ig),
r,
g,
b,
a,
s=0.04)
points_marker, lines, midpoint_sphere, first_point_text = rope_mrkrs
msg.markers.append(lines)
# msg.markers.append(points_marker)
# msg.markers.append(first_point_text)
if 'gripper' in state:
gripper = state['gripper']
gripper_sphere = make_gripper_marker(gripper,
next(ig),
r,
g,
b,
a,
label + '_gt_gripper',
Marker.SPHERE,
s=0.04)
msg.markers.append(gripper_sphere)
if add_predicted(rope_key_name) in state:
rope_points = np.reshape(state[add_predicted(rope_key_name)],
[-1, 3])
markers = make_rope_marker(rope_points,
'world', label + "_" + rope_key_name,
next(ig), r, g, b, a)
msg.markers.extend(markers)
if add_predicted('gripper') in state:
pred_gripper = state[add_predicted('gripper')]
gripper_sphere = make_gripper_marker(pred_gripper, next(ig), r, g,
b, a, label + "_gripper",
Marker.SPHERE)
msg.markers.append(gripper_sphere)
self.state_viz_pub.publish(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"))
z_offset = 0.05
gripper = np.reshape(get_maybe_predicted(data, 'gripper'),
[3]) + [0, 0, z_offset]
target_gripper = np.reshape(
get_maybe_predicted(data, 'gripper_position'),
[3]) + [0, 0, z_offset]
idx = kwargs.get("idx", 0)
msg = MarkerArray()
msg.markers.append(
rviz_arrow(position=gripper,
target_position=target_gripper,
r=r,
g=g,
b=b,
a=a,
idx=idx,
label=label,
**kwargs))
self.action_viz_pub.publish(msg)
def on_before_get_state_or_execute_action(self):
self.pos3d.register(
RegisterPosition3DControllerRequest(
scoped_link_name=self.ROPE_LINK_NAME,
controller_type='pid',
kp_vel=10.0,
kp_pos=10.0,
max_force=10.0,
max_vel=0.1))
def register_movable_object(self, scoped_link_name):
self.pos3d.register(
RegisterPosition3DControllerRequest(
scoped_link_name=scoped_link_name,
controller_type='pid',
kp_pos=50.0,
kp_vel=1000.0,
max_force=50.0,
max_vel=1.0))
def execute_action(self, action: Dict):
timeout_s = action.get('timeout_s', 1.0)
speed_mps = action.get('speed', 0.15)
pos_msg: Point = ros_numpy.msgify(Point, action['gripper_position'])
get_res = self.pos3d.get(scoped_link_name=self.ROPE_LINK_NAME)
pos_msg.z = get_res.pos.z
req = Position3DActionRequest(
scoped_link_name=self.ROPE_LINK_NAME,
position=pos_msg,
tolerance_m=0.005,
speed_mps=speed_mps,
timeout_s=timeout_s,
)
self.pos3d.move(req)
def sample_action(self,
action_rng: np.random.RandomState,
environment: Dict,
state: Dict,
action_params: Dict,
validate: bool,
stateless: Optional[bool] = False):
action = None
for _ in range(self.max_action_attempts):
# sample the previous action with 80% probability, this improves exploration
repeat_probability = action_params.get(
'repeat_delta_gripper_motion_probability', 0.8)
if self.last_action is not None and action_rng.uniform(
0, 1) < repeat_probability and not stateless:
gripper_delta_position = self.last_action[
'gripper_delta_position']
else:
theta = action_rng.uniform(-np.pi, np.pi)
displacement = action_rng.uniform(
0, action_params['max_distance_gripper_can_move'])
dx = np.cos(theta) * displacement
dy = np.sin(theta) * displacement
gripper_delta_position = np.array([dx, dy, 0])
gripper_position = state['gripper'] + gripper_delta_position
action = {
'gripper_position': gripper_position,
'gripper_delta_position': gripper_delta_position,
'timeout_s': action_params['dt'],
}
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 = self.gripper_out_of_bounds(action['gripper_position'],
action_params)
return not out_of_bounds
@staticmethod
def interpolate(start_state, end_state, step_size=0.05):
gripper_start = np.array(start_state['gripper'])
gripper_end = np.array(end_state['gripper'])
gripper_delta = gripper_end - gripper_start
steps = np.round(np.linalg.norm(gripper_delta) / step_size).astype(
np.int64)
interpolated_actions = []
for t in np.linspace(step_size, 1, steps):
gripper_t = gripper_start + gripper_delta * t
action = {
'gripper_position': gripper_t,
}
interpolated_actions.append(action)
return interpolated_actions
@staticmethod
def add_noise(action: Dict, noise_rng: np.random.RandomState):
gripper_noise = noise_rng.normal(scale=0.01, size=[3])
return {'gripper_position': action['gripper_position'] + gripper_noise}
def gripper_out_of_bounds(self, gripper, data_collection_params: Dict):
gripper_extent = data_collection_params['gripper_action_sample_extent']
return RopeDraggingScenario.is_out_of_bounds(gripper, 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
def get_state(self):
gripper_res = self.pos3d.get(scoped_link_name=self.ROPE_LINK_NAME)
assert gripper_res.success
rope_res = self.get_rope_srv(GetRopeStateRequest())
rope_state_vector = []
assert (len(rope_res.positions) == RopeDraggingScenario.n_links)
for p in rope_res.positions:
rope_state_vector.append(p.x)
rope_state_vector.append(p.y)
rope_state_vector.append(p.z)
return {
'gripper': ros_numpy.numpify(gripper_res.pos),
rope_key_name: np.array(rope_state_vector, np.float32),
}
@staticmethod
def states_description() -> Dict:
return {
'gripper': 3,
rope_key_name: RopeDraggingScenario.n_links * 3,
}
@staticmethod
def actions_description() -> Dict:
# should match the keys of the dict return from action_to_dataset_action
return {
'gripper_position': 3,
'timeout_s': 1,
}
@staticmethod
def distance_to_goal(state: Dict[str, np.ndarray], goal: np.ndarray):
"""
Uses the first point in the rope subspace as the thing which we want to move to goal
:param state: A dictionary of numpy arrays
:param goal: Assumed to be a point in 3D
:return:
"""
rope_points = np.reshape(state[rope_key_name], [-1, 3])
tail_point = rope_points[0]
distance = np.linalg.norm(tail_point - goal['tail'])
return distance
@staticmethod
def distance_to_goal_differentiable(state, goal):
rope_points = tf.reshape(state[rope_key_name], [-1, 3])
tail_point = rope_points[0]
distance = tf.linalg.norm(tail_point - goal)
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)
return model_error
@staticmethod
def distance_differentiable(s1, s2):
rope_points1 = tf.reshape(s1[rope_key_name], [-1, 3])
tail_point1 = rope_points1[0]
rope_points2 = tf.reshape(s2[rope_key_name], [-1, 3])
tail_point2 = rope_points2[0]
return tf.linalg.norm(tail_point1 - tail_point2)
@staticmethod
def state_to_points_for_cc(state: Dict):
return state[rope_key_name].reshape(-1, 3)
def sample_goal(self, environment: Dict, rng: np.random.RandomState,
planner_params: Dict):
goal_type = planner_params['goal_params']['goal_type']
if goal_type == 'tailpoint':
return self.sample_tailpoint_goal(environment, rng, planner_params)
else:
raise NotImplementedError(planner_params['goal_type'])
def sample_tailpoint_goal(self, environment: Dict,
rng: np.random.RandomState,
planner_params: Dict):
# add more inflating to reduce the number of truly unacheivable gols
env_inflated = inflate_tf_3d(
env=environment['env'],
radius_m=2 * 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)
p = rng.uniform(extent[:, 0], extent[:, 1])
goal = {'tail': p}
row, col, channel = 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 local_environment_center_differentiable(state):
return state['gripper']
@staticmethod
def simple_name():
return "rope dragging"
@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)
@staticmethod
def put_state_robot_frame(state: Dict):
return state
@staticmethod
def put_state_local_frame(state):
rope = state[rope_key_name]
rope_points_shape = rope.shape[:-1].as_list() + [-1, 3]
points = tf.reshape(rope, rope_points_shape)
center = state['gripper']
rope_points_local = points - tf.expand_dims(center, axis=-2)
gripper_local = state['gripper'] - center
# # project all z coordinates down to what we saw in simulation... the real fix for this is to use TF and have actually cordinate frames
# gripper_local = gripper_local * tf.constant([[1, 1, 0]], tf.float32) + tf.constant([[1, 1, 0.02]], tf.float32)
# rope_points_local = rope_points_local * tf.constant([[1, 1, 0]], tf.float32)+ tf.constant([[1, 1, 0.02]], tf.float32)
rope_local = tf.reshape(rope_points_local, rope.shape)
return {
'gripper': gripper_local,
rope_key_name: rope_local,
}
@staticmethod
def put_action_local_frame(state: Dict, action: Dict):
target_gripper_position = action['gripper_position']
current_gripper_point = state['gripper']
gripper_delta = target_gripper_position - current_gripper_point
return {
'gripper_delta': gripper_delta,
}
def randomization_initialization(self):
pass
def randomize_environment(self, env_rng, params: Dict):
# TODO: make scenarios take in a environment method,
# or make environment randomization methods take in the scenario,
# or just make scenarios more composable and have a few different (static) combinations that are hard-coded
self.pos3d.enable(
Position3DEnableRequest(scoped_link_name=self.ROPE_LINK_NAME,
enable=False))
return self.slide_obstacles(env_rng, params)
def slide_obstacles(self, env_rng, params: Dict):
# set random positions for all the objects
for object_name in params['objects']:
scoped_link_name = gz_scope(object_name, 'link_1')
get_res = self.pos3d.get(scoped_link_name=scoped_link_name)
pos_msg = get_res.pos
pos_msg.x = pos_msg.x + env_rng.uniform(-0.2, 0.2)
pos_msg.y = pos_msg.y + env_rng.uniform(-0.2, 0.2)
pos_msg.z = pos_msg.z + env_rng.uniform(-0.2, 0.2)
self.register_movable_object(scoped_link_name)
req = Position3DActionRequest(speed_mps=0.1,
scoped_link_name=scoped_link_name,
tolerance_m=0.01,
position=pos_msg)
self.pos3d.set(req)
wait_req = Position3DWaitRequest()
wait_req.timeout_s = 0.1
for object_name in params['objects']:
scoped_link_name = gz_scope(object_name, 'link_1')
wait_req.scoped_link_names.append(scoped_link_name)
self.pos3d.wait(wait_req)
for object_name in params['objects']:
scoped_link_name = gz_scope(object_name, 'link_1')
self.pos3d.enable(
Position3DEnableRequest(scoped_link_name=scoped_link_name,
enable=False))
@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()}
def index_action_time(self, action, t):
action_t = {}
for feature_name in ['gripper_position']:
if action[feature_name].is_batched:
if t < action[feature_name].shape[0]:
action_t[feature_name] = action[feature_name][t]
else:
action_t[feature_name] = action[feature_name][t - 1]
else:
if t < action[feature_name].shape[1]:
action_t[feature_name] = action[feature_name][:, t]
else:
action_t[feature_name] = action[feature_name][:, t - 1]
return action_t
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_goal_rviz(self,
goal: Dict,
goal_threshold: float,
actually_at_goal: Optional[bool] = None):
goal_marker_msg = MarkerArray()
tail_marker = Marker()
tail_marker.scale.x = goal_threshold * 2
tail_marker.scale.y = goal_threshold * 2
tail_marker.scale.z = goal_threshold * 2
tail_marker.action = Marker.ADD
tail_marker.type = Marker.SPHERE
tail_marker.header.frame_id = "world"
tail_marker.header.stamp = rospy.Time.now()
tail_marker.ns = 'goal'
tail_marker.id = 0
if actually_at_goal:
tail_marker.color.r = 0.4
tail_marker.color.g = 0.8
tail_marker.color.b = 0.4
tail_marker.color.a = 0.8
else:
tail_marker.color.r = 0.5
tail_marker.color.g = 0.3
tail_marker.color.b = 0.8
tail_marker.color.a = 0.8
tail_marker.pose.position.x = goal['tail'][0]
tail_marker.pose.position.y = goal['tail'][1]
tail_marker.pose.position.z = goal['tail'][2]
tail_marker.pose.orientation.w = 1
goal_marker_msg.markers.append(tail_marker)
self.state_viz_pub.publish(goal_marker_msg)
def get_environment(self, params: Dict, **kwargs):
res = params.get("res", 0.01)
return get_environment_for_extents_3d(
extent=params['extent'],
res=res,
service_provider=self.service_provider,
excluded_models=self.get_excluded_models_for_env())
def get_excluded_models_for_env(self):
return ['dragging_rope']
def detach_rope_from_gripper(self, rope_link_name: str):
enable_req = Position3DEnableRequest()
enable_req.scoped_link_name = gz_scope(self.ROPE_NAMESPACE,
rope_link_name)
enable_req.enable = False
self.pos3d.enable(enable_req)