def __init__(self,
n_substeps=1,
reward_type="dense",
observation_type="state",
headless=False,
image_size=[64, 64],
**kwargs):
"""Initializes a PegInHoleEnv object
:param args: arguments for agxViewer.
:param scene_path: path to binary file in assets/ folder containing serialized simulation defined in sim/ folder
:param n_substeps: number os simulation steps per call to step().
:param end_effectors: list of EndEffector objects, defining controllable constraints.
:param observation_config: ObservationConfig object, defining the types of observations.
:param camera_config: dictionary containing EYE, CENTER, UP information for rendering, with lighting info.
:param reward_config: reward configuration object, defines success condition and reward function.
"""
self.reward_type = reward_type
self.segment_pos_old = None
self.n_segments = None
self.headless = headless
camera_distance = 0.1 # meters
camera_config = CameraConfig(eye=agx.Vec3(0, -1.0, 0.2),
center=agx.Vec3(0, 0, 0.2),
up=agx.Vec3(0., 0., 1.0),
light_position=agx.Vec4(
0, -camera_distance, camera_distance,
1.),
light_direction=agx.Vec3(0., 0., -1.))
if 'agxViewer' in kwargs:
args = sys.argv + kwargs['agxViewer']
else:
args = sys.argv
# Change window size
args.extend(["--window", "400", "600"])
no_graphics = headless and observation_type not in ("rgb", "depth",
"rgb_and_depth")
# Disable rendering in headless mode
if headless:
args.extend(["--osgWindow", "0"])
if headless and observation_type == "gt":
# args.extend(["--osgWindow", "0"])
args.extend(["--agxOnly", "1", "--osgWindow", "0"])
super(PegInHoleEnv,
self).__init__(scene_path=SCENE_PATH,
n_substeps=n_substeps,
observation_type=observation_type,
n_actions=4,
camera_pose=camera_config.camera_pose,
no_graphics=no_graphics,
image_size=image_size,
args=args)
def post(self, t):
if self.textTable:
color = agx.Vec4(0.3, 0.6, 0.7, 1)
if self.text_color:
color = self.text_color
for i, v in enumerate(self.text_table):
self.app.getSceneDecorator().setText(i,
str(v[0]) + " " + v[1](),
color)
def add_rendering(sim):
camera_distance = 0.5
light_pos = agx.Vec4(LENGTH / 2, - camera_distance, camera_distance, 1.)
light_dir = agx.Vec3(0., 0., -1.)
app = agxOSG.ExampleApplication(sim)
app.setAutoStepping(True)
app.setEnableDebugRenderer(False)
app.setEnableOSGRenderer(True)
scene_decorator = app.getSceneDecorator()
light_source_0 = scene_decorator.getLightSource(agxOSG.SceneDecorator.LIGHT0)
light_source_0.setPosition(light_pos)
light_source_0.setDirection(light_dir)
root = app.getRoot()
rbs = sim.getRigidBodies()
for rb in rbs:
name = rb.getName()
node = agxOSG.createVisual(rb, root)
if name == "ground":
agxOSG.setDiffuseColor(node, agxRender.Color.Gray())
elif name == "pusher":
agxOSG.setDiffuseColor(node, agxRender.Color(0.0, 0.0, 1.0, 1.0))
elif "obstacle" in name:
agxOSG.setDiffuseColor(node, agxRender.Color(1.0, 0.0, 0.0, 1.0))
elif "dlo" in name:
agxOSG.setDiffuseColor(node, agxRender.Color(0.0, 1.0, 0.0, 1.0))
elif "bounding_box" in name:
agxOSG.setDiffuseColor(node, agxRender.Color.Burlywood())
else: # Base segments
agxOSG.setDiffuseColor(node, agxRender.Color.Beige())
agxOSG.setAlpha(node, 0.2)
if "goal" in name:
agxOSG.setAlpha(node, 0.2)
scene_decorator = app.getSceneDecorator()
light_source_0 = scene_decorator.getLightSource(agxOSG.SceneDecorator.LIGHT0)
light_source_0.setPosition(light_pos)
light_source_0.setDirection(light_dir)
scene_decorator.setEnableLogo(False)
return app
def add_rendering(sim):
camera_distance = 0.5
light_pos = agx.Vec4(CYLINDER_LENGTH / 2, -camera_distance,
camera_distance, 1.)
light_dir = agx.Vec3(0., 0., -1.)
app = agxOSG.ExampleApplication(sim)
app.setAutoStepping(False)
root = app.getRoot()
rbs = sim.getRigidBodies()
for rb in rbs:
name = rb.getName()
node = agxOSG.createVisual(rb, root, 2.0)
if "ring" in name:
agxOSG.setDiffuseColor(node, COLOR_RING)
elif "ground" in name:
agxOSG.setDiffuseColor(node, COLOR_GROUND)
elif "cylinder" in name:
agxOSG.setDiffuseColor(node, COLOR_CYLINDER)
elif "gripper_right" == name:
agxOSG.setDiffuseColor(node, agxRender.Color(0.0, 0.0, 1.0, 1.0))
elif "gripper_left" == name:
agxOSG.setDiffuseColor(node, agxRender.Color(1.0, 0.0, 0.0, 1.0))
else:
agxOSG.setDiffuseColor(node, agxRender.Color.Beige())
agxOSG.setAlpha(node, 0.2)
app.setEnableDebugRenderer(False)
app.setEnableOSGRenderer(True)
scene_decorator = app.getSceneDecorator()
light_source_0 = scene_decorator.getLightSource(
agxOSG.SceneDecorator.LIGHT0)
light_source_0.setPosition(light_pos)
light_source_0.setDirection(light_dir)
scene_decorator.setEnableLogo(False)
return app
def add_rendering(sim):
camera_distance = 0.5
light_pos = agx.Vec4(LENGTH / 2, -camera_distance, camera_distance, 1.)
light_dir = agx.Vec3(0., 0., -1.)
app = agxOSG.ExampleApplication(sim)
app.setAutoStepping(False)
app.setEnableDebugRenderer(False)
app.setEnableOSGRenderer(True)
root = app.getSceneRoot()
rbs = sim.getRigidBodies()
for rb in rbs:
name = rb.getName()
node = agxOSG.createVisual(rb, root)
if name == "ground":
agxOSG.setDiffuseColor(node, agxRender.Color.Gray())
elif "gripper_left" in name and "base" not in name:
agxOSG.setDiffuseColor(node, agxRender.Color.Red())
elif "gripper_right" in name and "base" not in name:
agxOSG.setDiffuseColor(node, agxRender.Color.Blue())
elif "dlo" in name:
agxOSG.setDiffuseColor(node, agxRender.Color.Green())
else:
agxOSG.setDiffuseColor(node, agxRender.Color.Beige())
agxOSG.setAlpha(node, 0.5)
if "goal" in name:
agxOSG.setAlpha(node, 0.2)
scene_decorator = app.getSceneDecorator()
light_source_0 = scene_decorator.getLightSource(
agxOSG.SceneDecorator.LIGHT0)
light_source_0.setPosition(light_pos)
light_source_0.setDirection(light_dir)
scene_decorator.setEnableLogo(False)
return app
def __init__(self,
n_substeps=1,
reward_type="dense",
observation_type="state",
headless=False,
image_size=[64, 64],
**kwargs):
"""Initializes a RubberBandEnv object
:param args: arguments for agxViewer.
:param scene_path: path to binary file in assets/ folder containing serialized simulation defined in sim/ folder
:param n_substeps: number os simulation steps per call to step().
:param end_effectors: list of EndEffector objects, defining controllable constraints.
:param observation_config: ObservationConfig object, defining the types of observations.
:param camera_config: dictionary containing EYE, CENTER, UP information for rendering, with lighting info.
:param reward_config: reward configuration object, defines success condition and reward function.
"""
self.reward_type = reward_type
self.observation_type = observation_type
self.segments_pos_old = None
self.headless = headless
camera_distance = 0.15 # meters
camera_config = CameraConfig(eye=agx.Vec3(0, -0.1, camera_distance),
center=agx.Vec3(0, 0, 0.0),
up=agx.Vec3(0., 0., 0.0),
light_position=agx.Vec4(
0, -camera_distance, camera_distance,
1.),
light_direction=agx.Vec3(0., 0., -1.))
gripper = EndEffector(
name='gripper',
controllable=True,
observable=True,
max_velocity=0.4, # m/s
max_acceleration=0.2 # m/s^2
)
gripper.add_constraint(
name='gripper_joint_base_x',
end_effector_dof=EndEffectorConstraint.Dof.X_TRANSLATION,
compute_forces_enabled=False,
velocity_control=True,
compliance_control=False)
gripper.add_constraint(
name='gripper_joint_base_y',
end_effector_dof=EndEffectorConstraint.Dof.Y_TRANSLATION,
compute_forces_enabled=False,
velocity_control=True,
compliance_control=False)
gripper.add_constraint(
name='gripper_joint_base_z',
end_effector_dof=EndEffectorConstraint.Dof.Z_TRANSLATION,
compute_forces_enabled=False,
velocity_control=True,
compliance_control=False)
self.end_effectors = [gripper]
if 'agxViewer' in kwargs:
args = sys.argv + kwargs['agxViewer']
else:
args = sys.argv
# Change window size
args.extend(["--window", "600", "600"])
no_graphics = headless and observation_type not in ("rgb", "depth",
"rgb_and_depth")
# Disable rendering in headless mode
if headless:
args.extend(["--osgWindow", "0"])
if headless and observation_type == "gt":
# args.extend(["--osgWindow", "0"])
args.extend(["--agxOnly", "1", "--osgWindow", "0"])
super(RubberBandEnv,
self).__init__(scene_path=SCENE_PATH,
n_substeps=n_substeps,
observation_type=observation_type,
n_actions=3,
image_size=image_size,
camera_pose=camera_config.camera_pose,
no_graphics=no_graphics,
args=args)
def __init__(self,
n_substeps,
observation_config=None,
pushers=None,
reward_type=None,
reward_config=None,
scene_path=None,
goal_scene_path=None,
**kwargs):
"""Initializes PushRope environment
:param int n_substeps: number of simulation steps between each action step
:param ObservationConfig: types of observations to be used
:param list pushers: EndEffector objects
:param RewardConfig.RewardType reward_type: type of reward
:param RewardConfig reward_config: adds possibility to completely override reward definition
:param str scene_path: possibility to overwrite default scene file
:param str goal_scene_path: possibility to overwrite default goal scene file
"""
camera_distance = 0.21 # meters
camera_config = CameraConfig(eye=agx.Vec3(0, -camera_distance / 2,
camera_distance),
center=agx.Vec3(0, 0, 0),
up=agx.Vec3(0., 0., 0.),
light_position=agx.Vec4(
0, -camera_distance / 2,
camera_distance * 0.9, 1.),
light_direction=agx.Vec3(0., 0., -1.))
if not pushers:
pusher = EndEffector(
name='pusher',
controllable=True,
observable=True,
max_velocity=5 / 100, # m/s
max_acceleration=10 / 100, # m/s^2
)
pusher.add_constraint(
name='pusher_joint_base_x',
end_effector_dof=EndEffectorConstraint.Dof.X_TRANSLATION,
compute_forces_enabled=False,
velocity_control=True,
compliance_control=False)
pusher.add_constraint(
name='pusher_joint_base_y',
end_effector_dof=EndEffectorConstraint.Dof.Y_TRANSLATION,
compute_forces_enabled=False,
velocity_control=True,
compliance_control=False)
pusher.add_constraint(
name='pusher_joint_base_z',
end_effector_dof=EndEffectorConstraint.Dof.Z_TRANSLATION,
compute_forces_enabled=False,
velocity_control=False,
compliance_control=False)
pushers = [pusher]
if not observation_config:
observation_config = ObservationConfig(
goals=[ObservationConfig.ObservationType.DLO_CURVATURE])
observation_config.set_dlo_frenet_curvature()
observation_config.set_ee_position()
if not reward_type:
reward_type = RewardConfig.RewardType.DENSE
if not reward_config:
reward_config = Reward(reward_type=reward_type,
reward_range=(-1.5, 1.5),
dlo_curvature_threshold=0.1)
if not scene_path:
scene_path = SCENE_PATH
if not goal_scene_path:
goal_scene_path = GOAL_SCENE_PATH
args = kwargs['agxViewer'] if 'agxViewer' in kwargs else sys.argv
show_goal = kwargs['show_goal'] if 'show_goal' in kwargs else False
osg_window = kwargs['osg_window'] if 'osg_window' in kwargs else False
agx_only = kwargs['agx_only'] if 'agx_only' in kwargs else False
randomized_goal = kwargs[
'randomized_goal'] if 'randomized_goal' in kwargs else False
# Overwrite goal_scene_path with starting point for random goals
if randomized_goal:
goal_scene_path = RANDOM_GOAL_SCENE_PATH
if not os.path.exists(SCENE_PATH):
raise IOError("File %s does not exist" % SCENE_PATH)
logger.info("Fetching environment from {}".format(SCENE_PATH))
super(PushRopeEnv,
self).__init__(args=args,
scene_path=scene_path,
n_substeps=n_substeps,
end_effectors=pushers,
observation_config=observation_config,
camera_config=camera_config,
reward_config=reward_config,
randomized_goal=randomized_goal,
goal_scene_path=goal_scene_path,
show_goal=show_goal,
osg_window=osg_window,
agx_only=agx_only)
def __init__(self, n_substeps, observation_config=None, grippers=None, reward_type=None, reward_config=None,
scene_path=None, goal_scene_path=None, dof_vector=None, **kwargs):
"""Initializes BendWire environment
:param int n_substeps: number of simulation steps between each action step
:param ObservationConfig: types of observations to be used
:param list grippers: EndEffector objects
:param RewardConfig.RewardType reward_type: type of reward
:param RewardConfig reward_config: adds possibility to completely override reward definition
:param str scene_path: possibility to overwrite default scene file
:param str goal_scene_path: possibility to overwrite default goal scene file
:param np.array dof_vector: desired gripper(s) degrees of freedom for generating random goal , [x, y, z]
"""
length = 0.1 # meters
camera_distance = 0.5 # meters
camera_config = CameraConfig(
eye=agx.Vec3(length / 2, -4 * length, 0),
center=agx.Vec3(length / 2, 0, 0),
up=agx.Vec3(0., 0., 1.),
light_position=agx.Vec4(length / 2, - camera_distance, camera_distance, 1.),
light_direction=agx.Vec3(0., 0., -1.)
)
if not grippers:
gripper_right = EndEffector(
name='gripper_right',
controllable=True,
observable=True,
max_velocity=10 / 1000, # m/s
max_acceleration=10 / 1000, # m/s^2
)
gripper_right.add_constraint(name='gripper_right_joint_base_x',
end_effector_dof=EndEffectorConstraint.Dof.X_TRANSLATION,
compute_forces_enabled=False,
velocity_control=True,
compliance_control=False)
gripper_right.add_constraint(name='gripper_right_joint_base_y',
end_effector_dof=EndEffectorConstraint.Dof.Y_TRANSLATION,
compute_forces_enabled=False,
velocity_control=True,
compliance_control=False)
gripper_right.add_constraint(name='gripper_right_joint_base_z',
end_effector_dof=EndEffectorConstraint.Dof.Z_TRANSLATION,
compute_forces_enabled=False,
velocity_control=True,
compliance_control=False)
gripper_right.add_constraint(name='hinge_joint_right',
end_effector_dof=EndEffectorConstraint.Dof.Y_ROTATION,
compute_forces_enabled=False,
velocity_control=False,
compliance_control=False)
grippers = [gripper_right]
if not observation_config:
observation_config = ObservationConfig(goals=[ObservationConfig.ObservationType.DLO_CURVATURE,
ObservationConfig.ObservationType.EE_VELOCITY])
observation_config.set_dlo_frenet_curvature()
if not reward_config:
if not reward_type:
reward_type = RewardConfig.RewardType.DENSE
reward_config = Reward(reward_type=reward_type, reward_range=(-1.5, 1.5), set_done_on_success=False,
dlo_curvature_threshold=0.05)
if not scene_path:
scene_path = SCENE_PATH
if not goal_scene_path:
goal_scene_path = GOAL_SCENE_PATH
args = kwargs['agxViewer'] if 'agxViewer' in kwargs else sys.argv
show_goal = kwargs['show_goal'] if 'show_goal' in kwargs else False
osg_window = kwargs['osg_window'] if 'osg_window' in kwargs else False
agx_only = kwargs['agx_only'] if 'agx_only' in kwargs else False
randomized_goal = kwargs['randomized_goal'] if 'randomized_goal' in kwargs else False
# Overwrite goal_scene_path with starting point for random goals
if randomized_goal:
goal_scene_path = RANDOM_GOAL_SCENE_PATH
if not os.path.exists(SCENE_PATH):
raise IOError("File %s does not exist" % SCENE_PATH)
logger.info("Fetching environment from {}".format(SCENE_PATH))
# Randomization of goal can be changed using dof_vector
if dof_vector:
self.dof_vector = dof_vector
else:
self.dof_vector = np.ones(3)
super(BendWireEnv, self).__init__(args=args,
scene_path=scene_path,
n_substeps=n_substeps,
end_effectors=grippers,
observation_config=observation_config,
camera_config=camera_config,
reward_config=reward_config,
randomized_goal=randomized_goal,
goal_scene_path=goal_scene_path,
show_goal=show_goal,
osg_window=osg_window,
agx_only=agx_only)