def __init__(self,
env: iGibsonEnv = None,
base_mp_algo: str = 'birrt',
arm_mp_algo: str = 'birrt',
optimize_iter: int = 0,
fine_motion_plan: bool = True):
"""
Get planning related parameters.
"""
self.env = env
assert 'occupancy_grid' in self.env.output
# get planning related parameters from env
self.robot_id = self.env.robots[0].robot_ids[0]
# self.mesh_id = self.scene.mesh_body_id
# mesh id should not be used
self.map_size = self.env.scene.trav_map_original_size * \
self.env.scene.trav_map_default_resolution
self.grid_resolution = self.env.grid_resolution
self.occupancy_range = self.env.sensors['scan_occ'].occupancy_range
self.robot_footprint_radius = self.env.sensors[
'scan_occ'].robot_footprint_radius
self.robot_footprint_radius_in_map = self.env.sensors[
'scan_occ'].robot_footprint_radius_in_map
self.robot = self.env.robots[0]
self.base_mp_algo = base_mp_algo
self.arm_mp_algo = arm_mp_algo
self.base_mp_resolutions = np.array([0.05, 0.05, 0.05])
self.optimize_iter = optimize_iter
self.mode = self.env.mode
self.initial_height = self.env.initial_pos_z_offset
self.fine_motion_plan = fine_motion_plan
self.robot_type = self.env.config['robot']
if self.env.simulator.viewer is not None:
self.env.simulator.viewer.setup_motion_planner(self)
if self.robot_type in ['Fetch', 'Movo']:
self.setup_arm_mp()
self.arm_interaction_length = 0.2
self.marker = None
self.marker_direction = None
if self.mode in ['gui', 'iggui']:
self.marker = VisualMarker(radius=0.04, rgba_color=[0, 0, 1, 1])
self.marker_direction = VisualMarker(visual_shape=p.GEOM_CAPSULE,
radius=0.01,
length=0.2,
initial_offset=[0, 0, -0.1],
rgba_color=[0, 0, 1, 1])
self.env.simulator.import_object(self.marker, use_pbr=False)
self.env.simulator.import_object(self.marker_direction,
use_pbr=False)
def create_visual_object(self):
"""
Create visual objects to visualize interaction
"""
self.constraint_marker = VisualMarker(radius=0.04,
rgba_color=[0, 0, 1, 1])
self.constraint_marker2 = VisualMarker(visual_shape=p.GEOM_CAPSULE,
radius=0.01,
length=3,
initial_offset=[0, 0, -1.5],
rgba_color=[0, 0, 1, 1])
if self.simulator is not None:
self.simulator.import_object(self.constraint_marker2,
use_pbr=False)
self.simulator.import_object(self.constraint_marker, use_pbr=False)
self.constraint_marker.set_position([0, 0, -1])
self.constraint_marker2.set_position([0, 0, -1])
def load_visualization(self, env):
"""
Load visualization, such as initial and target position, shortest path, etc
:param env: environment instance
"""
if env.mode != 'gui':
return
cyl_length = 0.2
self.initial_pos_vis_obj = VisualMarker(
visual_shape=p.GEOM_CYLINDER,
rgba_color=[1, 0, 0, 0.3],
radius=self.dist_tol,
length=cyl_length,
initial_offset=[0, 0, cyl_length / 2.0])
self.target_pos_vis_obj = VisualMarker(
visual_shape=p.GEOM_CYLINDER,
rgba_color=[0, 0, 1, 0.3],
radius=self.dist_tol,
length=cyl_length,
initial_offset=[0, 0, cyl_length / 2.0])
if self.target_visual_object_visible_to_agent:
env.simulator.import_object(self.initial_pos_vis_obj)
env.simulator.import_object(self.target_pos_vis_obj)
else:
self.initial_pos_vis_obj.load()
self.target_pos_vis_obj.load()
if env.scene.build_graph:
self.num_waypoints_vis = 250
self.waypoints_vis = [
VisualMarker(visual_shape=p.GEOM_CYLINDER,
rgba_color=[0, 1, 0, 0.3],
radius=0.1,
length=cyl_length,
initial_offset=[0, 0, cyl_length / 2.0])
for _ in range(self.num_waypoints_vis)
]
for waypoint in self.waypoints_vis:
waypoint.load()
class PointNavFixedTask(BaseTask):
"""
Point Nav Fixed Task
The goal is to navigate to a fixed goal position
"""
def __init__(self, env):
super(PointNavFixedTask, self).__init__(env)
self.reward_type = self.config.get('reward_type', 'l2')
self.termination_conditions = [
MaxCollision(self.config),
Timeout(self.config),
OutOfBound(self.config),
PointGoal(self.config),
]
self.reward_functions = [
PotentialReward(self.config),
CollisionReward(self.config),
PointGoalReward(self.config),
]
self.initial_pos = np.array(self.config.get('initial_pos', [0, 0, 0]))
self.initial_orn = np.array(self.config.get('initial_orn', [0, 0, 0]))
self.target_pos = np.array(self.config.get('target_pos', [5, 5, 0]))
self.goal_format = self.config.get('goal_format', 'polar')
self.dist_tol = self.termination_conditions[-1].dist_tol
self.visual_object_at_initial_target_pos = self.config.get(
'visual_object_at_initial_target_pos', True)
self.target_visual_object_visible_to_agent = self.config.get(
'target_visual_object_visible_to_agent', False)
self.floor_num = 0
self.load_visualization(env)
def load_visualization(self, env):
"""
Load visualization, such as initial and target position, shortest path, etc
:param env: environment instance
"""
if env.mode != 'gui':
return
cyl_length = 0.2
self.initial_pos_vis_obj = VisualMarker(
visual_shape=p.GEOM_CYLINDER,
rgba_color=[1, 0, 0, 0.3],
radius=self.dist_tol,
length=cyl_length,
initial_offset=[0, 0, cyl_length / 2.0])
self.target_pos_vis_obj = VisualMarker(
visual_shape=p.GEOM_CYLINDER,
rgba_color=[0, 0, 1, 0.3],
radius=self.dist_tol,
length=cyl_length,
initial_offset=[0, 0, cyl_length / 2.0])
if self.target_visual_object_visible_to_agent:
env.simulator.import_object(self.initial_pos_vis_obj)
env.simulator.import_object(self.target_pos_vis_obj)
else:
self.initial_pos_vis_obj.load()
self.target_pos_vis_obj.load()
if env.scene.build_graph:
self.num_waypoints_vis = 250
self.waypoints_vis = [
VisualMarker(visual_shape=p.GEOM_CYLINDER,
rgba_color=[0, 1, 0, 0.3],
radius=0.1,
length=cyl_length,
initial_offset=[0, 0, cyl_length / 2.0])
for _ in range(self.num_waypoints_vis)
]
for waypoint in self.waypoints_vis:
waypoint.load()
def get_geodesic_potential(self, env):
"""
Get potential based on geodesic distance
:param env: environment instance
:return: geodesic distance to the target position
"""
_, geodesic_dist = self.get_shortest_path(env)
return geodesic_dist
def get_l2_potential(self, env):
"""
Get potential based on L2 distance
:param env: environment instance
:return: L2 distance to the target position
"""
return l2_distance(env.robots[0].get_position()[:2],
self.target_pos[:2])
def get_potential(self, env):
"""
Compute task-specific potential: distance to the goal
:param env: environment instance
:return: task potential
"""
if self.reward_type == 'l2':
return self.get_l2_potential(env)
elif self.reward_type == 'geodesic':
return self.get_geodesic_potential(env)
def reset_scene(self, env):
"""
Task-specific scene reset: reset scene objects or floor plane
:param env: environment instance
"""
if isinstance(env.scene, InteractiveIndoorScene):
env.scene.reset_scene_objects()
elif isinstance(env.scene, StaticIndoorScene):
env.scene.reset_floor(floor=self.floor_num)
def reset_agent(self, env):
"""
Task-specific agent reset: land the robot to initial pose, compute initial potential
:param env: environment instance
"""
env.land(env.robots[0], self.initial_pos, self.initial_orn)
self.path_length = 0.0
self.robot_pos = self.initial_pos[:2]
self.geodesic_dist = self.get_geodesic_potential(env)
for reward_function in self.reward_functions:
reward_function.reset(self, env)
def get_termination(self, env, collision_links=[], action=None, info={}):
"""
Aggreate termination conditions and fill info
"""
done, info = super(PointNavFixedTask,
self).get_termination(env, collision_links, action,
info)
info['path_length'] = self.path_length
if done:
info['spl'] = float(info['success']) * \
min(1.0, self.geodesic_dist / self.path_length)
else:
info['spl'] = 0.0
return done, info
def global_to_local(self, env, pos):
"""
Convert a 3D point in global frame to agent's local frame
:param env: environment instance
:param pos: a 3D point in global frame
:return: the same 3D point in agent's local frame
"""
return rotate_vector_3d(pos - env.robots[0].get_position(),
*env.robots[0].get_rpy())
def get_task_obs(self, env):
"""
Get task-specific observation, including goal position, current velocities, etc.
:param env: environment instance
:return: task-specific observation
"""
task_obs = self.global_to_local(env, self.target_pos)[:2]
if self.goal_format == 'polar':
task_obs = np.array(cartesian_to_polar(task_obs[0], task_obs[1]))
# linear velocity along the x-axis
linear_velocity = rotate_vector_3d(env.robots[0].get_linear_velocity(),
*env.robots[0].get_rpy())[0]
# angular velocity along the z-axis
angular_velocity = rotate_vector_3d(
env.robots[0].get_angular_velocity(), *env.robots[0].get_rpy())[2]
task_obs = np.append(task_obs, [linear_velocity, angular_velocity])
return task_obs
def get_shortest_path(self,
env,
from_initial_pos=False,
entire_path=False):
"""
Get the shortest path and geodesic distance from the robot or the initial position to the target position
:param env: environment instance
:param from_initial_pos: whether source is initial position rather than current position
:param entire_path: whether to return the entire shortest path
:return: shortest path and geodesic distance to the target position
"""
if from_initial_pos:
source = self.initial_pos[:2]
else:
source = env.robots[0].get_position()[:2]
target = self.target_pos[:2]
return env.scene.get_shortest_path(self.floor_num,
source,
target,
entire_path=entire_path)
def step_visualization(self, env):
"""
Step visualization
:param env: environment instance
"""
if env.mode != 'gui':
return
self.initial_pos_vis_obj.set_position(self.initial_pos)
self.target_pos_vis_obj.set_position(self.target_pos)
if env.scene.build_graph:
shortest_path, _ = self.get_shortest_path(env, entire_path=True)
floor_height = env.scene.get_floor_height(self.floor_num)
num_nodes = min(self.num_waypoints_vis, shortest_path.shape[0])
for i in range(num_nodes):
self.waypoints_vis[i].set_position(pos=np.array(
[shortest_path[i][0], shortest_path[i][1], floor_height]))
for i in range(num_nodes, self.num_waypoints_vis):
self.waypoints_vis[i].set_position(
pos=np.array([0.0, 0.0, 100.0]))
def step(self, env):
"""
Perform task-specific step: step visualization and aggregate path length
:param env: environment instance
"""
self.step_visualization(env)
new_robot_pos = env.robots[0].get_position()[:2]
self.path_length += l2_distance(self.robot_pos, new_robot_pos)
self.robot_pos = new_robot_pos
class Viewer:
def __init__(
self,
initial_pos=[0, 0, 1.2],
initial_view_direction=[1, 0, 0],
initial_up=[0, 0, 1],
simulator=None,
renderer=None,
min_cam_z=-1e6,
):
"""
iGibson GUI (Viewer) for navigation, manipulation and motion planning / execution
:param initial_pos: position of the camera
:param initial_view_direction: viewing direction of the camera
:param initial_up: up direction
:param simulator: iGibson simulator
:param renderer: iGibson renderer
:param min_cam_z: minimum camera z
"""
self.px = initial_pos[0]
self.py = initial_pos[1]
self.pz = initial_pos[2]
self.theta = np.arctan2(initial_view_direction[1],
initial_view_direction[0])
self.phi = np.arctan2(
initial_view_direction[2],
np.sqrt(initial_view_direction[0]**2 +
initial_view_direction[1]**2))
self.min_cam_z = min_cam_z
self.show_help = 0
self._mouse_ix, self._mouse_iy = -1, -1
self.left_down = False
self.middle_down = False
self.right_down = False
self.view_direction = np.array(initial_view_direction)
self.up = initial_up
self.renderer = renderer
self.simulator = simulator
self.cid = []
# Flag to control if the mouse interface is in navigation, manipulation
# or motion planning/execution mode
self.manipulation_mode = 0
# Video recording
self.recording = False # Boolean if we are recording frames from the viewer
self.pause_recording = False # Flag to pause/resume recording
self.video_folder = ""
cv2.namedWindow('ExternalView')
cv2.moveWindow("ExternalView", 0, 0)
cv2.namedWindow('RobotView')
cv2.setMouseCallback('ExternalView', self.mouse_callback)
self.create_visual_object()
self.planner = None
self.block_command = False
def setup_motion_planner(self, planner=None):
"""
Store the motion planner that is passed in
:param planner: motion planner
"""
self.planner = planner
def create_visual_object(self):
"""
Create visual objects to visualize interaction
"""
self.constraint_marker = VisualMarker(radius=0.04,
rgba_color=[0, 0, 1, 1])
self.constraint_marker2 = VisualMarker(visual_shape=p.GEOM_CAPSULE,
radius=0.01,
length=3,
initial_offset=[0, 0, -1.5],
rgba_color=[0, 0, 1, 1])
if self.simulator is not None:
self.simulator.import_object(self.constraint_marker2,
use_pbr=False)
self.simulator.import_object(self.constraint_marker, use_pbr=False)
self.constraint_marker.set_position([0, 0, -1])
self.constraint_marker2.set_position([0, 0, -1])
def apply_push_force(self, x, y, force):
"""
Apply pushing force to a 3D point. Given a pixel location (x, y),
compute the 3D location of that point, and then apply a virtual force
of a given magnitude towards the negative surface normal at that point
:param x: image pixel x coordinate
:param y: image pixel y coordinate
:param force: force magnitude
"""
camera_pose = np.array([self.px, self.py, self.pz])
self.renderer.set_camera(camera_pose,
camera_pose + self.view_direction, self.up)
position_cam = np.array([
(x - self.renderer.width / 2) / float(self.renderer.width / 2) *
np.tan(self.renderer.horizontal_fov / 2.0 / 180.0 * np.pi),
-(y - self.renderer.height / 2) / float(self.renderer.height / 2) *
np.tan(self.renderer.vertical_fov / 2.0 / 180.0 * np.pi), -1, 1
])
position_cam[:3] *= 5
position_world = np.linalg.inv(self.renderer.V).dot(position_cam)
position_eye = camera_pose
res = p.rayTest(position_eye, position_world[:3])
if len(res) > 0 and res[0][0] != -1:
# there is hit
object_id, link_id, _, hit_pos, hit_normal = res[0]
p.changeDynamics(object_id,
-1,
activationState=p.ACTIVATION_STATE_WAKE_UP)
p.applyExternalForce(object_id, link_id,
-np.array(hit_normal) * force, hit_pos,
p.WORLD_FRAME)
def create_constraint(self, x, y, fixed=False):
"""
Create a constraint between the constraint marker and the object
at pixel location (x, y). This is used for human users' mouse
interaction with the objects in the scenes.
:param x: image pixel x coordinate
:param y: image pixel y coordinate
:param fixed: whether to create a fixed joint. Otherwise, it's a point2point joint.
"""
camera_pose = np.array([self.px, self.py, self.pz])
self.renderer.set_camera(camera_pose,
camera_pose + self.view_direction, self.up)
position_cam = np.array([
(x - self.renderer.width / 2) / float(self.renderer.width / 2) *
np.tan(self.renderer.horizontal_fov / 2.0 / 180.0 * np.pi),
-(y - self.renderer.height / 2) / float(self.renderer.height / 2) *
np.tan(self.renderer.vertical_fov / 2.0 / 180.0 * np.pi), -1, 1
])
position_cam[:3] *= 5
position_world = np.linalg.inv(self.renderer.V).dot(position_cam)
position_eye = camera_pose
res = p.rayTest(position_eye, position_world[:3])
if len(res) > 0 and res[0][0] != -1:
object_id, link_id, _, hit_pos, hit_normal = res[0]
p.changeDynamics(object_id,
-1,
activationState=p.ACTIVATION_STATE_WAKE_UP)
link_pos, link_orn = None, None
if link_id == -1:
link_pos, link_orn = p.getBasePositionAndOrientation(object_id)
else:
link_state = p.getLinkState(object_id, link_id)
link_pos, link_orn = link_state[:2]
child_frame_pos, child_frame_orn = \
p.multiplyTransforms(*p.invertTransform(link_pos, link_orn),
hit_pos,
[0, 0, 0, 1])
self.constraint_marker.set_position(hit_pos)
self.constraint_marker2.set_position(hit_pos)
self.dist = np.linalg.norm(np.array(hit_pos) - camera_pose)
cid = p.createConstraint(
parentBodyUniqueId=self.constraint_marker.body_id,
parentLinkIndex=-1,
childBodyUniqueId=object_id,
childLinkIndex=link_id,
jointType=[p.JOINT_POINT2POINT, p.JOINT_FIXED][fixed],
jointAxis=(0, 0, 0),
parentFramePosition=(0, 0, 0),
childFramePosition=child_frame_pos,
childFrameOrientation=child_frame_orn,
)
p.changeConstraint(cid, maxForce=100)
self.cid.append(cid)
self.interaction_x, self.interaction_y = x, y
def get_hit(self, x, y):
"""
Shoot a ray through pixel location (x, y) and returns the position and
normal that this ray hits
:param x: image pixel x coordinate
:param y: image pixel y coordinate
"""
camera_pose = np.array([self.px, self.py, self.pz])
self.renderer.set_camera(camera_pose,
camera_pose + self.view_direction, self.up)
position_cam = np.array([
(x - self.renderer.width / 2) / float(self.renderer.width / 2) *
np.tan(self.renderer.horizontal_fov / 2.0 / 180.0 * np.pi),
-(y - self.renderer.height / 2) / float(self.renderer.height / 2) *
np.tan(self.renderer.vertical_fov / 2.0 / 180.0 * np.pi), -1, 1
])
position_cam[:3] *= 5
position_world = np.linalg.inv(self.renderer.V).dot(position_cam)
position_eye = camera_pose
res = p.rayTest(position_eye, position_world[:3])
hit_pos = None
hit_normal = None
if len(res) > 0 and res[0][0] != -1:
object_id, link_id, _, hit_pos, hit_normal = res[0]
return hit_pos, hit_normal
def remove_constraint(self):
"""
Remove constraints created by create_constraint
"""
for cid in self.cid:
p.removeConstraint(cid)
self.cid = []
self.constraint_marker.set_position([0, 0, 100])
self.constraint_marker2.set_position([0, 0, 100])
def move_constraint(self, x, y):
"""
Move the constraint marker (when the mouse is moved during interaction)
:param x: image pixel x coordinate
:param y: image pixel y coordinate
"""
camera_pose = np.array([self.px, self.py, self.pz])
self.renderer.set_camera(camera_pose,
camera_pose + self.view_direction, self.up)
position_cam = np.array([
(x - self.renderer.width / 2) / float(self.renderer.width / 2) *
np.tan(self.renderer.horizontal_fov / 2.0 / 180.0 * np.pi),
-(y - self.renderer.height / 2) / float(self.renderer.height / 2) *
np.tan(self.renderer.vertical_fov / 2.0 / 180.0 * np.pi), -1, 1
])
position_cam[:3] = position_cam[:3] / \
np.linalg.norm(position_cam[:3]) * self.dist
position_world = np.linalg.inv(self.renderer.V).dot(position_cam)
position_world /= position_world[3]
self.constraint_marker.set_position(position_world[:3])
self.constraint_marker2.set_position(position_world[:3])
self.interaction_x, self.interaction_y = x, y
def move_constraint_z(self, dy):
"""
Move the constraint marker closer or further away from the camera
(when the mouse is moved during interaction)
:param dy: delta y coordinate in the pixel space
"""
x, y = self.interaction_x, self.interaction_y
camera_pose = np.array([self.px, self.py, self.pz])
self.renderer.set_camera(camera_pose,
camera_pose + self.view_direction, self.up)
self.dist *= (1 - dy)
if self.dist < 0.1:
self.dist = 0.1
position_cam = np.array([
(x - self.renderer.width / 2) / float(self.renderer.width / 2) *
np.tan(self.renderer.horizontal_fov / 2.0 / 180.0 * np.pi),
-(y - self.renderer.height / 2) / float(self.renderer.height / 2) *
np.tan(self.renderer.vertical_fov / 2.0 / 180.0 * np.pi), -1, 1
])
position_cam[:3] = position_cam[:3] / \
np.linalg.norm(position_cam[:3]) * self.dist
position_world = np.linalg.inv(self.renderer.V).dot(position_cam)
position_world /= position_world[3]
self.constraint_marker.set_position(position_world[:3])
self.constraint_marker2.set_position(position_world[:3])
def mouse_callback(self, event, x, y, flags, params):
"""
Mouse callback that handles all the mouse events
:param event: OpenCV mouse event
:param x: image pixel x coordinate
:param y: image pixel y coordinate
:param flags: any relevant flags passed by OpenCV.
:param params: any extra parameters supplied by OpenCV
"""
# Navigation mode
if self.manipulation_mode == 0:
# Only once, when pressing left mouse while ctrl key is pressed
if flags == cv2.EVENT_FLAG_LBUTTON + cv2.EVENT_FLAG_CTRLKEY and not self.right_down:
self._mouse_ix, self._mouse_iy = x, y
self.right_down = True
# Middle mouse button press or only once, when pressing left
# mouse while shift key is pressed (Mac compatibility)
elif (event == cv2.EVENT_MBUTTONDOWN) or (
flags == cv2.EVENT_FLAG_LBUTTON + cv2.EVENT_FLAG_SHIFTKEY
and not self.middle_down):
self._mouse_ix, self._mouse_iy = x, y
self.middle_down = True
# left mouse button press
elif event == cv2.EVENT_LBUTTONDOWN:
self._mouse_ix, self._mouse_iy = x, y
self.left_down = True
# left mouse button released
elif event == cv2.EVENT_LBUTTONUP:
self.left_down = False
self.right_down = False
self.middle_down = False
# middle mouse button released
elif event == cv2.EVENT_MBUTTONUP:
self.middle_down = False
# moving mouse location on the window
if event == cv2.EVENT_MOUSEMOVE:
# if left button was pressed we change orientation of camera
if self.left_down:
dx = (x - self._mouse_ix) / 100.0
dy = (y - self._mouse_iy) / 100.0
self._mouse_ix = x
self._mouse_iy = y
if not ((flags & cv2.EVENT_FLAG_CTRLKEY
and flags & cv2.EVENT_FLAG_SHIFTKEY) or
(flags & cv2.EVENT_FLAG_CTRLKEY
and flags & cv2.EVENT_FLAG_ALTKEY)):
self.phi += dy
self.phi = np.clip(self.phi, -np.pi / 2 + 1e-5,
np.pi / 2 - 1e-5)
self.theta += dx
self.view_direction = np.array([
np.cos(self.theta) * np.cos(self.phi),
np.sin(self.theta) * np.cos(self.phi),
np.sin(self.phi)
])
# if middle button was pressed we get closer/further away in the viewing direction
elif self.middle_down:
d_vd = (y - self._mouse_iy) / 100.0
self._mouse_iy = y
motion_along_vd = d_vd * self.view_direction
self.px += motion_along_vd[0]
self.py += motion_along_vd[1]
self.pz += motion_along_vd[2]
self.pz = max(self.min_cam_z, self.pz)
# if right button was pressed we change translation of camera
elif self.right_down:
zz = self.view_direction / \
np.linalg.norm(self.view_direction)
xx = np.cross(zz, np.array([0, 0, 1]))
xx = xx / np.linalg.norm(xx)
yy = np.cross(xx, zz)
motion_along_vx = -((x - self._mouse_ix) / 100.0) * xx
motion_along_vy = ((y - self._mouse_iy) / 100.0) * yy
self._mouse_ix = x
self._mouse_iy = y
self.px += (motion_along_vx[0] + motion_along_vy[0])
self.py += (motion_along_vx[1] + motion_along_vy[1])
self.pz += (motion_along_vx[2] + motion_along_vy[2])
self.pz = max(self.min_cam_z, self.pz)
# Manipulation mode
elif self.manipulation_mode == 1:
# Middle mouse button press or only once, when pressing left mouse
# while shift key is pressed (Mac compatibility)
if (event == cv2.EVENT_MBUTTONDOWN) or (
flags == cv2.EVENT_FLAG_LBUTTON + cv2.EVENT_FLAG_SHIFTKEY
and not self.middle_down):
self._mouse_ix, self._mouse_iy = x, y
self.middle_down = True
self.create_constraint(x, y, fixed=True)
elif event == cv2.EVENT_LBUTTONDOWN: # left mouse button press
self._mouse_ix, self._mouse_iy = x, y
self.left_down = True
self.create_constraint(x, y, fixed=False)
elif event == cv2.EVENT_LBUTTONUP: # left mouse button released
self.left_down = False
self.right_down = False
self.middle_down = False
self.remove_constraint()
elif event == cv2.EVENT_MBUTTONUP: # middle mouse button released
self.left_down = False
self.right_down = False
self.middle_down = False
self.remove_constraint()
if event == cv2.EVENT_MOUSEMOVE: # moving mouse location on the window
if (self.left_down or self.middle_down
) and not flags & cv2.EVENT_FLAG_CTRLKEY:
self._mouse_ix = x
self._mouse_iy = y
self.move_constraint(x, y)
elif (self.left_down
or self.middle_down) and flags & cv2.EVENT_FLAG_CTRLKEY:
dy = (y - self._mouse_iy) / 500.0
self.move_constraint_z(dy)
# Motion planning / execution mode
elif self.manipulation_mode == 2 and not self.block_command:
# left mouse button press
if event == cv2.EVENT_LBUTTONDOWN:
self._mouse_ix, self._mouse_iy = x, y
self.left_down = True
self.hit_pos, _ = self.get_hit(x, y)
# Base motion
if event == cv2.EVENT_LBUTTONUP:
hit_pos, _ = self.get_hit(x, y)
target_yaw = np.arctan2(hit_pos[1] - self.hit_pos[1],
hit_pos[0] - self.hit_pos[0])
self.planner.set_marker_position_yaw(self.hit_pos, target_yaw)
self.left_down = False
if hit_pos is not None:
self.block_command = True
plan = self.planner.plan_base_motion(
[self.hit_pos[0], self.hit_pos[1], target_yaw])
if plan is not None and len(plan) > 0:
self.planner.dry_run_base_plan(plan)
self.block_command = False
# Visualize base subgoal orientation
if event == cv2.EVENT_MOUSEMOVE:
if self.left_down:
hit_pos, _ = self.get_hit(x, y)
target_yaw = np.arctan2(hit_pos[1] - self.hit_pos[1],
hit_pos[0] - self.hit_pos[0])
self.planner.set_marker_position_yaw(
self.hit_pos, target_yaw)
# Arm motion
if event == cv2.EVENT_MBUTTONDOWN:
hit_pos, hit_normal = self.get_hit(x, y)
if hit_pos is not None:
self.block_command = True
plan = self.planner.plan_arm_push(hit_pos,
-np.array(hit_normal))
self.planner.execute_arm_push(plan, hit_pos,
-np.array(hit_normal))
self.block_command = False
def show_help_text(self, frame):
"""
Show help text
"""
if self.show_help < 0:
return
if self.show_help >= 150:
first_color = (255, 0, 0)
help_text = "Keyboard cheatsheet:"
cv2.putText(frame, help_text, (10, 80), cv2.FONT_HERSHEY_SIMPLEX,
0.5, first_color, 1, cv2.LINE_AA)
help_text = "'w','a','s','d': up, left, down, right (any mode)"
cv2.putText(frame, help_text, (10, 100), cv2.FONT_HERSHEY_SIMPLEX,
0.5, first_color, 1, cv2.LINE_AA)
help_text = "'q','e': turn left, turn right (any mode)"
cv2.putText(frame, help_text, (10, 120), cv2.FONT_HERSHEY_SIMPLEX,
0.5, first_color, 1, cv2.LINE_AA)
help_text = "'m': switch control mode across navigation, manipulation, and planning"
cv2.putText(frame, help_text, (10, 140), cv2.FONT_HERSHEY_SIMPLEX,
0.5, first_color, 1, cv2.LINE_AA)
help_text = "'r': Start/stop recording frames (results in \\tmp folder)"
cv2.putText(frame, help_text, (10, 160), cv2.FONT_HERSHEY_SIMPLEX,
0.5, first_color, 1, cv2.LINE_AA)
help_text = "'p': Pause/resume recording"
cv2.putText(frame, help_text, (10, 180), cv2.FONT_HERSHEY_SIMPLEX,
0.5, first_color, 1, cv2.LINE_AA)
help_text = "'h': Show this help on screen"
cv2.putText(frame, help_text, (10, 200), cv2.FONT_HERSHEY_SIMPLEX,
0.5, first_color, 1, cv2.LINE_AA)
help_text = "'ESC': Quit"
cv2.putText(frame, help_text, (10, 220), cv2.FONT_HERSHEY_SIMPLEX,
0.5, first_color, 1, cv2.LINE_AA)
else:
second_color = (255, 0, 255)
help_text = "Mouse control in navigation mode:"
cv2.putText(frame, help_text, (10, 80), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = "Left click and drag: rotate camera"
cv2.putText(frame, help_text, (10, 100), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = "CTRL + left click and drag: translate camera"
cv2.putText(frame, help_text, (10, 120), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = "Middle click and drag (linux) or left SHIFT + left click and drag:"
cv2.putText(frame, help_text, (10, 140), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = "translate camera closer/further away in the viewing direction"
cv2.putText(frame, help_text, (10, 160), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = "Mouse control in manipulation mode:"
cv2.putText(frame, help_text, (10, 180), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = "Left click and drag: create ball-joint connection to clicked object and move it"
cv2.putText(frame, help_text, (10, 200), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = "Middle click and drag (linux) or left SHIFT + left click and drag: create rigid connection"
cv2.putText(frame, help_text, (10, 220), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = " to object and move it"
cv2.putText(frame, help_text, (10, 240), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = "CTRL + click and drag: up/down of the mouse moves object further/closer"
cv2.putText(frame, help_text, (10, 260), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = "Mouse control in planning mode:"
cv2.putText(frame, help_text, (10, 280), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = "Left click: create (click), visualize (drag) and plan / execute (release) a base motion subgoal"
cv2.putText(frame, help_text, (10, 300), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = "for the robot base to reach the physical point that corresponds to the clicked pixel"
cv2.putText(frame, help_text, (10, 320), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = "Middle click: create, and plan / execute an arm motion subgoal"
cv2.putText(frame, help_text, (10, 340), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
help_text = "for the robot end-effector to reach the physical point that corresponds to the clicked pixel"
cv2.putText(frame, help_text, (10, 360), cv2.FONT_HERSHEY_SIMPLEX,
0.5, second_color, 1, cv2.LINE_AA)
self.show_help -= 1
def update(self):
"""
Update images of Viewer
"""
camera_pose = np.array([self.px, self.py, self.pz])
if self.renderer is not None:
self.renderer.set_camera(camera_pose,
camera_pose + self.view_direction,
self.up)
if self.renderer is not None:
frame = cv2.cvtColor(
np.concatenate(self.renderer.render(modes=('rgb')), axis=1),
cv2.COLOR_RGB2BGR)
else:
frame = np.zeros((300, 300, 3)).astype(np.uint8)
# Text with the position and viewing direction of the camera of the external viewer
text_color = (0, 0, 0)
cv2.putText(
frame,
"px {:1.1f} py {:1.1f} pz {:1.1f}".format(self.px, self.py,
self.pz), (10, 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, text_color, 1, cv2.LINE_AA)
cv2.putText(frame,
"[{:1.1f} {:1.1f} {:1.1f}]".format(*self.view_direction),
(10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.5, text_color, 1,
cv2.LINE_AA)
cv2.putText(frame, ["nav mode", "manip mode",
"planning mode"][self.manipulation_mode], (10, 60),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, text_color, 1, cv2.LINE_AA)
self.show_help_text(frame)
cv2.imshow('ExternalView', frame)
# We keep some double functinality for "backcompatibility"
q = cv2.waitKey(1)
move_vec = self.view_direction[:2]
# step size is 0.05m
move_vec = move_vec / np.linalg.norm(move_vec) * 0.05
# show help text
if q == ord('h'):
self.show_help = 300
# move
elif q in [ord('w'), ord('s'), ord('a'), ord('d')]:
if q == ord('w'):
yaw = 0.0
elif q == ord('s'):
yaw = np.pi
elif q == ord('a'):
yaw = -np.pi / 2.0
elif q == ord('d'):
yaw = np.pi / 2.0
move_vec = rotate_vector_2d(move_vec, yaw)
self.px += move_vec[0]
self.py += move_vec[1]
if self.manipulation_mode == 1:
self.move_constraint(self._mouse_ix, self._mouse_iy)
# turn left
elif q == ord('q'):
self.theta += np.pi / 36
self.view_direction = np.array([
np.cos(self.theta) * np.cos(self.phi),
np.sin(self.theta) * np.cos(self.phi),
np.sin(self.phi)
])
if self.manipulation_mode == 1:
self.move_constraint(self._mouse_ix, self._mouse_iy)
# turn right
elif q == ord('e'):
self.theta -= np.pi / 36
self.view_direction = np.array([
np.cos(self.theta) * np.cos(self.phi),
np.sin(self.theta) * np.cos(self.phi),
np.sin(self.phi)
])
if self.manipulation_mode == 1:
self.move_constraint(self._mouse_ix, self._mouse_iy)
# quit (Esc)
elif q == 27:
if self.video_folder != "":
logging.info(
"You recorded a video. To compile the frames into a mp4 go to the corresponding subfolder"
+ " in /tmp and execute: ")
logging.info(
"ffmpeg -i %5d.png -y -c:a copy -c:v libx264 -crf 18 -preset veryslow -r 30 video.mp4"
)
logging.info(
"The last folder you collected images for a video was: " +
self.video_folder)
exit()
# Start/Stop recording. Stopping saves frames to files
elif q == ord('r'):
if self.recording:
self.recording = False
self.pause_recording = False
else:
logging.info("Start recording*****************************")
# Current time string to use to save the temporal urdfs
timestr = time.strftime("%Y%m%d-%H%M%S")
# Create the subfolder
self.video_folder = os.path.join(
"/tmp", '{}_{}_{}'.format(timestr, random.getrandbits(64),
os.getpid()))
os.makedirs(self.video_folder, exist_ok=True)
self.recording = True
self.frame_idx = 0
# Pause/Resume recording
elif q == ord('p'):
if self.pause_recording:
self.pause_recording = False
else:
self.pause_recording = True
# Switch amoung navigation, manipulation, motion planning / execution modes
elif q == ord('m'):
self.left_down = False
self.middle_down = False
self.right_down = False
self.manipulation_mode = (self.manipulation_mode + 1) % 3
if self.recording and not self.pause_recording:
cv2.imwrite(
os.path.join(self.video_folder,
'{:05d}.png'.format(self.frame_idx)),
(frame * 255).astype(np.uint8))
self.frame_idx += 1
if self.renderer is not None:
frames = self.renderer.render_robot_cameras(modes=('rgb'))
if len(frames) > 0:
frame = cv2.cvtColor(np.concatenate(frames, axis=1),
cv2.COLOR_RGB2BGR)
cv2.imshow('RobotView', frame)
def render_physics_gifs(main_urdf_file_and_offset):
step_per_sec = 100
s = Simulator(mode='headless',
image_width=512,
image_height=512,
physics_timestep=1 / float(step_per_sec))
root_dir = '/cvgl2/u/chengshu/ig_dataset_v5/objects'
obj_count = 0
for i, obj_class_dir in enumerate(sorted(os.listdir(root_dir))):
obj_class = obj_class_dir
# if obj_class not in SELECTED_CLASSES:
# continue
obj_class_dir = os.path.join(root_dir, obj_class_dir)
for obj_inst_dir in os.listdir(obj_class_dir):
# if obj_inst_dir != '14402':
# continue
imgs = []
scene = EmptyScene()
s.import_scene(scene, render_floor_plane=True)
obj_inst_name = obj_inst_dir
# urdf_path = obj_inst_name + '.urdf'
obj_inst_dir = os.path.join(obj_class_dir, obj_inst_dir)
urdf_path, offset = main_urdf_file_and_offset[obj_inst_dir]
# urdf_path = os.path.join(obj_inst_dir, urdf_path)
# print('urdf_path', urdf_path)
obj = ArticulatedObject(urdf_path)
s.import_object(obj)
push_visual_marker = VisualMarker(radius=0.1)
s.import_object(push_visual_marker)
push_visual_marker.set_position([100, 100, 0.0])
z = stable_z_on_aabb(obj.body_id, [[0, 0, 0], [0, 0, 0]])
# offset is translation from the bounding box center to the base link frame origin
# need to add another offset that is the translation from the base link frame origin to the inertial frame origin
# p.resetBasePositionAndOrientation() sets the inertial frame origin instead of the base link frame origin
# Assuming the bounding box center is at (0, 0, z) where z is half of the extent in z-direction
base_link_to_inertial = p.getDynamicsInfo(obj.body_id, -1)[3]
obj.set_position([
offset[0] + base_link_to_inertial[0],
offset[1] + base_link_to_inertial[1], z
])
center, extent = get_center_extent(obj.body_id)
# the bounding box center should be at (0, 0) on the xy plane and the bottom of the bounding box should touch the ground
if not (np.linalg.norm(center[:2]) < 1e-3
and np.abs(center[2] - extent[2] / 2.0) < 1e-3):
print('-' * 50)
print('WARNING:', urdf_path, 'xy error',
np.linalg.norm(center[:2]), 'z error',
np.abs(center[2] - extent[2] / 2.0))
height = extent[2]
max_half_extent = max(extent[0], extent[1]) / 2.0
px = max_half_extent * 2
py = max_half_extent * 2
pz = extent[2] * 1.2
camera_pose = np.array([px, py, pz])
# camera_pose = np.array([0.01, 0.01, 3])
s.renderer.set_camera(camera_pose, [0, 0, 0], [0, 0, 1])
# drop 1 second
for _ in range(step_per_sec):
s.step()
frame = s.renderer.render(modes=('rgb'))
imgs.append(
Image.fromarray(
(frame[0][:, :, :3] * 255).astype(np.uint8)))
ray_start = [max_half_extent * 1.5, max_half_extent * 1.5, 0]
ray_end = [-100.0, -100.0, 0]
unit_force = np.array([-1.0, -1.0, 0.0])
unit_force /= np.linalg.norm(unit_force)
force_mag = 100.0
ray_zs = [height * 0.5]
valid_ray_z = False
for trial in range(5):
for ray_z in ray_zs:
ray_start[2] = ray_z
ray_end[2] = ray_z
res = p.rayTest(ray_start, ray_end)
if res[0][0] == obj.body_id:
valid_ray_z = True
break
if valid_ray_z:
break
increment = 1 / (2**(trial + 1))
ray_zs = np.arange(increment / 2.0, 1.0, increment) * height
# push 4 seconds
for i in range(step_per_sec * 4):
res = p.rayTest(ray_start, ray_end)
object_id, link_id, _, hit_pos, hit_normal = res[0]
if object_id != obj.body_id:
break
push_visual_marker.set_position(hit_pos)
p.applyExternalForce(object_id, link_id,
unit_force * force_mag, hit_pos,
p.WORLD_FRAME)
s.step()
# print(hit_pos)
frame = s.renderer.render(modes=('rgb'))
rgb = frame[0][:, :, :3]
# add red border
border_width = 10
border_color = np.array([1.0, 0.0, 0.0])
rgb[:border_width, :, :] = border_color
rgb[-border_width:, :, :] = border_color
rgb[:, :border_width, :] = border_color
rgb[:, -border_width:, :] = border_color
imgs.append(Image.fromarray((rgb * 255).astype(np.uint8)))
gif_path = '{}/visualizations/{}_cm_physics_v3.gif'.format(
obj_inst_dir, obj_inst_name)
imgs = imgs[::4]
imgs[0].save(gif_path,
save_all=True,
append_images=imgs[1:],
optimize=True,
duration=40,
loop=0)
obj_count += 1
# print(obj_count, gif_path, len(imgs), valid_ray_z, ray_z)
print(obj_count, gif_path)
s.reload()
class MotionPlanningWrapper(object):
"""
Motion planner wrapper that supports both base and arm motion
"""
def __init__(self,
env: iGibsonEnv = None,
base_mp_algo: str = 'birrt',
arm_mp_algo: str = 'birrt',
optimize_iter: int = 0,
fine_motion_plan: bool = True):
"""
Get planning related parameters.
"""
self.env = env
assert 'occupancy_grid' in self.env.output
# get planning related parameters from env
self.robot_id = self.env.robots[0].robot_ids[0]
# self.mesh_id = self.scene.mesh_body_id
# mesh id should not be used
self.map_size = self.env.scene.trav_map_original_size * \
self.env.scene.trav_map_default_resolution
self.grid_resolution = self.env.grid_resolution
self.occupancy_range = self.env.sensors['scan_occ'].occupancy_range
self.robot_footprint_radius = self.env.sensors[
'scan_occ'].robot_footprint_radius
self.robot_footprint_radius_in_map = self.env.sensors[
'scan_occ'].robot_footprint_radius_in_map
self.robot = self.env.robots[0]
self.base_mp_algo = base_mp_algo
self.arm_mp_algo = arm_mp_algo
self.base_mp_resolutions = np.array([0.05, 0.05, 0.05])
self.optimize_iter = optimize_iter
self.mode = self.env.mode
self.initial_height = self.env.initial_pos_z_offset
self.fine_motion_plan = fine_motion_plan
self.robot_type = self.env.config['robot']
if self.env.simulator.viewer is not None:
self.env.simulator.viewer.setup_motion_planner(self)
if self.robot_type in ['Fetch', 'Movo']:
self.setup_arm_mp()
self.arm_interaction_length = 0.2
self.marker = None
self.marker_direction = None
if self.mode in ['gui', 'iggui']:
self.marker = VisualMarker(radius=0.04, rgba_color=[0, 0, 1, 1])
self.marker_direction = VisualMarker(visual_shape=p.GEOM_CAPSULE,
radius=0.01,
length=0.2,
initial_offset=[0, 0, -0.1],
rgba_color=[0, 0, 1, 1])
self.env.simulator.import_object(self.marker, use_pbr=False)
self.env.simulator.import_object(self.marker_direction,
use_pbr=False)
def set_marker_position(self, pos):
"""
Set subgoal marker position
:param pos: position
"""
self.marker.set_position(pos)
def set_marker_position_yaw(self, pos, yaw):
"""
Set subgoal marker position and orientation
:param pos: position
:param yaw: yaw angle
"""
quat = quatToXYZW(seq='wxyz', orn=euler.euler2quat(0, -np.pi / 2, yaw))
self.marker.set_position(pos)
self.marker_direction.set_position_orientation(pos, quat)
def set_marker_position_direction(self, pos, direction):
"""
Set subgoal marker position and orientation
:param pos: position
:param direction: direction vector
"""
yaw = np.arctan2(direction[1], direction[0])
self.set_marker_position_yaw(pos, yaw)
def setup_arm_mp(self):
"""
Set up arm motion planner
"""
if self.robot_type == 'Fetch':
self.arm_default_joint_positions = (0.10322468280792236,
-1.414019864768982,
1.5178184935241699,
0.8189625336474915,
2.200358942909668,
2.9631312579803466,
-1.2862852996643066,
0.0008453550418615341)
self.arm_joint_ids = joints_from_names(self.robot_id, [
'torso_lift_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'upperarm_roll_joint',
'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint',
'wrist_roll_joint'
])
elif self.robot_type == 'Movo':
self.arm_default_joint_positions = (0.205, -1.50058731470836,
-1.3002625076695704,
0.5204845864369407,
-2.6923805472917626,
-0.02678584326934146,
0.5065742552588746,
-1.562883631882778)
self.arm_joint_ids = joints_from_names(self.robot_id, [
"linear_joint",
"right_shoulder_pan_joint",
"right_shoulder_lift_joint",
"right_arm_half_joint",
"right_elbow_joint",
"right_wrist_spherical_1_joint",
"right_wrist_spherical_2_joint",
"right_wrist_3_joint",
])
self.arm_joint_ids_all = get_moving_links(self.robot_id,
self.arm_joint_ids)
self.arm_joint_ids_all = [
item for item in self.arm_joint_ids_all
if item != self.robot.end_effector_part_index()
]
self.arm_ik_threshold = 0.05
self.mp_obstacles = []
if type(self.env.scene) == StaticIndoorScene:
if self.env.scene.mesh_body_id is not None:
self.mp_obstacles.append(self.env.scene.mesh_body_id)
elif type(self.env.scene) == InteractiveIndoorScene:
self.mp_obstacles.extend(self.env.scene.get_body_ids())
def plan_base_motion(self, goal):
"""
Plan base motion given a base subgoal
:param goal: base subgoal
:return: waypoints or None if no plan can be found
"""
if self.marker is not None:
self.set_marker_position_yaw([goal[0], goal[1], 0.05], goal[2])
state = self.env.get_state()
x, y, theta = goal
grid = state['occupancy_grid']
yaw = self.robot.get_rpy()[2]
half_occupancy_range = self.occupancy_range / 2.0
robot_position_xy = self.robot.get_position()[:2]
corners = [
robot_position_xy + rotate_vector_2d(local_corner, -yaw)
for local_corner in [
np.array([half_occupancy_range, half_occupancy_range]),
np.array([half_occupancy_range, -half_occupancy_range]),
np.array([-half_occupancy_range, half_occupancy_range]),
np.array([-half_occupancy_range, -half_occupancy_range]),
]
]
path = plan_base_motion_2d(
self.robot_id, [x, y, theta],
(tuple(np.min(corners, axis=0)), tuple(np.max(corners, axis=0))),
map_2d=grid,
occupancy_range=self.occupancy_range,
grid_resolution=self.grid_resolution,
robot_footprint_radius_in_map=self.robot_footprint_radius_in_map,
resolutions=self.base_mp_resolutions,
obstacles=[],
algorithm=self.base_mp_algo,
optimize_iter=self.optimize_iter)
return path
def simulator_sync(self):
"""Sync the simulator to renderer"""
self.env.simulator.sync()
def simulator_step(self):
"""Step the simulator and sync the simulator to renderer"""
self.env.simulator.step()
self.simulator_sync()
def dry_run_base_plan(self, path):
"""
Dry run base motion plan by setting the base positions without physics simulation
:param path: base waypoints or None if no plan can be found
"""
if path is not None:
if self.mode in ['gui', 'iggui', 'pbgui']:
for way_point in path:
set_base_values_with_z(
self.robot_id,
[way_point[0], way_point[1], way_point[2]],
z=self.initial_height)
self.simulator_sync()
# sleep(0.005) # for animation
else:
set_base_values_with_z(self.robot_id,
[path[-1][0], path[-1][1], path[-1][2]],
z=self.initial_height)
def get_ik_parameters(self):
"""
Get IK parameters such as joint limits, joint damping, reset position, etc
:return: IK parameters
"""
max_limits, min_limits, rest_position, joint_range, joint_damping = None, None, None, None, None
if self.robot_type == 'Fetch':
max_limits = [0., 0.] + \
get_max_limits(self.robot_id, self.arm_joint_ids)
min_limits = [0., 0.] + \
get_min_limits(self.robot_id, self.arm_joint_ids)
# increase torso_lift_joint lower limit to 0.02 to avoid self-collision
min_limits[2] += 0.02
rest_position = [0., 0.] + \
list(get_joint_positions(self.robot_id, self.arm_joint_ids))
joint_range = list(np.array(max_limits) - np.array(min_limits))
joint_range = [item + 1 for item in joint_range]
joint_damping = [0.1 for _ in joint_range]
elif self.robot_type == 'Movo':
max_limits = get_max_limits(self.robot_id, self.robot.all_joints)
min_limits = get_min_limits(self.robot_id, self.robot.all_joints)
rest_position = list(
get_joint_positions(self.robot_id, self.robot.all_joints))
joint_range = list(np.array(max_limits) - np.array(min_limits))
joint_range = [item + 1 for item in joint_range]
joint_damping = [0.1 for _ in joint_range]
return (max_limits, min_limits, rest_position, joint_range,
joint_damping)
def get_arm_joint_positions(self, arm_ik_goal):
"""
Attempt to find arm_joint_positions that satisfies arm_subgoal
If failed, return None
:param arm_ik_goal: [x, y, z] in the world frame
:return: arm joint positions
"""
ik_start = time()
max_limits, min_limits, rest_position, joint_range, joint_damping = \
self.get_ik_parameters()
n_attempt = 0
max_attempt = 75
sample_fn = get_sample_fn(self.robot_id, self.arm_joint_ids)
base_pose = get_base_values(self.robot_id)
state_id = p.saveState()
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, False)
# find collision-free IK solution for arm_subgoal
while n_attempt < max_attempt:
if self.robot_type == 'Movo':
self.robot.tuck()
set_joint_positions(self.robot_id, self.arm_joint_ids, sample_fn())
arm_joint_positions = p.calculateInverseKinematics(
self.robot_id,
self.robot.end_effector_part_index(),
targetPosition=arm_ik_goal,
# targetOrientation=self.robots[0].get_orientation(),
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=joint_range,
restPoses=rest_position,
jointDamping=joint_damping,
solver=p.IK_DLS,
maxNumIterations=100)
if self.robot_type == 'Fetch':
arm_joint_positions = arm_joint_positions[2:10]
elif self.robot_type == 'Movo':
arm_joint_positions = arm_joint_positions[:8]
set_joint_positions(self.robot_id, self.arm_joint_ids,
arm_joint_positions)
dist = l2_distance(self.robot.get_end_effector_position(),
arm_ik_goal)
# print('dist', dist)
if dist > self.arm_ik_threshold:
n_attempt += 1
continue
# need to simulator_step to get the latest collision
self.simulator_step()
# simulator_step will slightly move the robot base and the objects
set_base_values_with_z(self.robot_id,
base_pose,
z=self.initial_height)
# self.reset_object_states()
# TODO: have a princpled way for stashing and resetting object states
# arm should not have any collision
if self.robot_type == 'Movo':
collision_free = is_collision_free(
body_a=self.robot_id, link_a_list=self.arm_joint_ids_all)
# ignore linear link
else:
collision_free = is_collision_free(
body_a=self.robot_id, link_a_list=self.arm_joint_ids)
if not collision_free:
n_attempt += 1
# print('arm has collision')
continue
# gripper should not have any self-collision
collision_free = is_collision_free(
body_a=self.robot_id,
link_a_list=[self.robot.end_effector_part_index()],
body_b=self.robot_id)
if not collision_free:
n_attempt += 1
print('gripper has collision')
continue
#self.episode_metrics['arm_ik_time'] += time() - ik_start
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, True)
p.restoreState(state_id)
p.removeState(state_id)
return arm_joint_positions
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, True)
p.restoreState(state_id)
p.removeState(state_id)
#self.episode_metrics['arm_ik_time'] += time() - ik_start
return None
def plan_arm_motion(self, arm_joint_positions):
"""
Attempt to reach arm arm_joint_positions and return arm trajectory
If failed, reset the arm to its original pose and return None
:param arm_joint_positions: final arm joint position to reach
:return: arm trajectory or None if no plan can be found
"""
disabled_collisions = {}
if self.robot_type == 'Fetch':
disabled_collisions = {
(link_from_name(self.robot_id, 'torso_lift_link'),
link_from_name(self.robot_id, 'torso_fixed_link')),
(link_from_name(self.robot_id, 'torso_lift_link'),
link_from_name(self.robot_id, 'shoulder_lift_link')),
(link_from_name(self.robot_id, 'torso_lift_link'),
link_from_name(self.robot_id, 'upperarm_roll_link')),
(link_from_name(self.robot_id, 'torso_lift_link'),
link_from_name(self.robot_id, 'forearm_roll_link')),
(link_from_name(self.robot_id, 'torso_lift_link'),
link_from_name(self.robot_id, 'elbow_flex_link'))
}
elif self.robot_type == 'Movo':
disabled_collisions = {
(link_from_name(self.robot_id, 'linear_actuator_link'),
link_from_name(self.robot_id, 'right_shoulder_link')),
(link_from_name(self.robot_id, 'right_base_link'),
link_from_name(self.robot_id, 'linear_actuator_fixed_link')),
(link_from_name(self.robot_id, 'linear_actuator_link'),
link_from_name(self.robot_id, 'right_arm_half_1_link')),
(link_from_name(self.robot_id, 'linear_actuator_link'),
link_from_name(self.robot_id, 'right_arm_half_2_link')),
(link_from_name(self.robot_id, 'linear_actuator_link'),
link_from_name(self.robot_id, 'right_forearm_link')),
(link_from_name(self.robot_id, 'linear_actuator_link'),
link_from_name(self.robot_id,
'right_wrist_spherical_1_link')),
(link_from_name(self.robot_id, 'linear_actuator_link'),
link_from_name(self.robot_id,
'right_wrist_spherical_2_link')),
(link_from_name(self.robot_id, 'linear_actuator_link'),
link_from_name(self.robot_id, 'right_wrist_3_link')),
(link_from_name(self.robot_id, 'right_wrist_spherical_2_link'),
link_from_name(self.robot_id, 'right_robotiq_coupler_link')),
(link_from_name(self.robot_id, 'right_shoulder_link'),
link_from_name(self.robot_id, 'linear_actuator_fixed_link')),
(link_from_name(self.robot_id, 'left_base_link'),
link_from_name(self.robot_id, 'linear_actuator_fixed_link')),
(link_from_name(self.robot_id, 'left_shoulder_link'),
link_from_name(self.robot_id, 'linear_actuator_fixed_link')),
(link_from_name(self.robot_id, 'left_arm_half_2_link'),
link_from_name(self.robot_id, 'linear_actuator_fixed_link')),
(link_from_name(self.robot_id, 'right_arm_half_2_link'),
link_from_name(self.robot_id, 'linear_actuator_fixed_link')),
(link_from_name(self.robot_id, 'right_arm_half_1_link'),
link_from_name(self.robot_id, 'linear_actuator_fixed_link')),
(link_from_name(self.robot_id, 'left_arm_half_1_link'),
link_from_name(self.robot_id, 'linear_actuator_fixed_link')),
}
if self.fine_motion_plan:
self_collisions = True
mp_obstacles = self.mp_obstacles
else:
self_collisions = False
mp_obstacles = []
plan_arm_start = time()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, False)
state_id = p.saveState()
allow_collision_links = []
if self.robot_type == 'Fetch':
allow_collision_links = [19]
elif self.robot_type == 'Movo':
allow_collision_links = [23, 24]
arm_path = plan_joint_motion(
self.robot_id,
self.arm_joint_ids,
arm_joint_positions,
disabled_collisions=disabled_collisions,
self_collisions=self_collisions,
obstacles=mp_obstacles,
algorithm=self.arm_mp_algo,
allow_collision_links=allow_collision_links,
)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, True)
p.restoreState(state_id)
p.removeState(state_id)
return arm_path
def dry_run_arm_plan(self, arm_path):
"""
Dry run arm motion plan by setting the arm joint position without physics simulation
:param arm_path: arm trajectory or None if no plan can be found
"""
base_pose = get_base_values(self.robot_id)
if arm_path is not None:
if self.mode in ['gui', 'iggui', 'pbgui']:
for joint_way_point in arm_path:
set_joint_positions(self.robot_id, self.arm_joint_ids,
joint_way_point)
set_base_values_with_z(self.robot_id,
base_pose,
z=self.initial_height)
self.simulator_sync()
# sleep(0.02) # animation
else:
set_joint_positions(self.robot_id, self.arm_joint_ids,
arm_path[-1])
else:
# print('arm mp fails')
if self.robot_type == 'Movo':
self.robot.tuck()
set_joint_positions(self.robot_id, self.arm_joint_ids,
self.arm_default_joint_positions)
def plan_arm_push(self, hit_pos, hit_normal):
"""
Attempt to reach a 3D position and prepare for a push later
:param hit_pos: 3D position to reach
:param hit_normal: direction to push after reacehing that position
:return: arm trajectory or None if no plan can be found
"""
if self.marker is not None:
self.set_marker_position_direction(hit_pos, hit_normal)
joint_positions = self.get_arm_joint_positions(hit_pos)
#print('planned JP', joint_positions)
set_joint_positions(self.robot_id, self.arm_joint_ids,
self.arm_default_joint_positions)
self.simulator_sync()
if joint_positions is not None:
plan = self.plan_arm_motion(joint_positions)
return plan
else:
return None
def interact(self, push_point, push_direction):
"""
Move the arm starting from the push_point along the push_direction
and physically simulate the interaction
:param push_point: 3D point to start pushing from
:param push_direction: push direction
"""
push_vector = np.array(push_direction) * self.arm_interaction_length
max_limits, min_limits, rest_position, joint_range, joint_damping = \
self.get_ik_parameters()
base_pose = get_base_values(self.robot_id)
steps = 50
for i in range(steps):
push_goal = np.array(push_point) + \
push_vector * (i + 1) / float(steps)
joint_positions = p.calculateInverseKinematics(
self.robot_id,
self.robot.end_effector_part_index(),
targetPosition=push_goal,
# targetOrientation=self.robots[0].get_orientation(),
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=joint_range,
restPoses=rest_position,
jointDamping=joint_damping,
solver=p.IK_DLS,
maxNumIterations=100)
if self.robot_type == 'Fetch':
joint_positions = joint_positions[2:10]
elif self.robot_type == 'Movo':
joint_positions = joint_positions[:8]
control_joints(self.robot_id, self.arm_joint_ids, joint_positions)
# set_joint_positions(self.robot_id, self.arm_joint_ids, joint_positions)
achieved = self.robot.get_end_effector_position()
# print('ee delta', np.array(achieved) - push_goal, np.linalg.norm(np.array(achieved) - push_goal))
# if self.robot_type == 'Movo':
# self.robot.control_tuck_left()
self.simulator_step()
set_base_values_with_z(self.robot_id,
base_pose,
z=self.initial_height)
if self.mode in ['pbgui', 'iggui', 'gui']:
sleep(0.02) # for visualization
def execute_arm_push(self, plan, hit_pos, hit_normal):
"""
Execute arm push given arm trajectory
Should be called after plan_arm_push()
:param plan: arm trajectory or None if no plan can be found
:param hit_pos: 3D position to reach
:param hit_normal: direction to push after reacehing that position
"""
if plan is not None:
self.dry_run_arm_plan(plan)
self.interact(hit_pos, hit_normal)
set_joint_positions(self.robot_id, self.arm_joint_ids,
self.arm_default_joint_positions)
self.simulator_sync()