Env¶
This module contains classes for defining an environment.
- class clt_core.env.BulletEnv(robot, name='', params={})¶
Class implementing the clutter env in pyBullet.
- Parameters
name (str) – A string with the name of the environment.
params (dict) – A dictionary with parameters for the environment.
- clear_target_occlusion(eps=0.003)¶
Checks if an obstacle is above the target object and occludes it. Then it removes it from the arena.
- reset()¶
Resets the environment. Basically it creates the initial state of the MDP.
- Returns
The observation after the reset.
- Return type
dict
- seed(seed=None)¶
Implement it for seeding the random generators of the environment, if it implements any stochasticity.
- Parameters
seed (int) – The seed
- step(action)¶
Moves the environment one step forward in time.
- Parameters
action – An action to perform.
- Returns
The observation after the step.
- Return type
dict
- step_linear(action)¶
Moves the environment one step forward in time.
- Parameters
action (tuple) – A tuple of two 3D np.arrays corresponding to the initial and final 3D point of the push with respect to inertia frame (workspace frame)
- Returns
A dictionary with the following keys: rgb, depth, seg, full_state. See get_obs() for more info.
- Return type
dict
- workspace2world(pos=None, quat=None, inv=False)¶
Transforms a pose in workspace coordinates to world coordinates
- Parameters
pos (list) – The position in workspace coordinates
quat (Quaternion) – The quaternion in workspace coordinates
- Returns
list (position in worldcreate_scene coordinates)
Quaternion (quaternion in world coordinates)
- class clt_core.env.RearrangementEnv(robot=<class 'clt_core.env.UR5Bullet'>, name='', params={})¶
Class implementing the clutter env in pyBullet.
- Parameters
name (str) – A string with the name of the environment.
params (dict) – A dictionary with parameters for the environment.
- clear_target_occlusion(eps=0.003)¶
Checks if an obstacle is above the target object and occludes it. Then it removes it from the arena.
- reset()¶
Resets the environment. Basically it creates the initial state of the MDP.
- Returns
The observation after the reset.
- Return type
dict
- seed(seed=None)¶
Implement it for seeding the random generators of the environment, if it implements any stochasticity.
- Parameters
seed (int) – The seed
- step(action)¶
Moves the environment one step forward in time.
- Parameters
action (tuple) – A tuple of two 3D np.arrays corresponding to the initial and final 3D point of the push with respect to inertia frame (workspace frame)
- Returns
A dictionary with the following keys: rgb, depth, seg, full_state. See get_obs() for more info.
- Return type
dict
- workspace2world(pos=None, quat=None, inv=False)¶
Transforms a pose in workspace coordinates to world coordinates
- Parameters
pos (list) – The position in workspace coordinates
quat (Quaternion) – The quaternion in workspace coordinates
- Returns
list (position in worldcreate_scene coordinates)
Quaternion (quaternion in world coordinates)
- class clt_core.env.UR5Bullet¶
- get_joint_position()¶
Returns the positions of the robot’s joints.
- Returns
A list of floats in rad for the joint positions of the robot.
- Return type
list
- get_joint_velocity()¶
Returns the velocities of the robot’s joints.
- Returns
A list of floats in rad/sec for the joint velocities of the robot.
- Return type
list
- get_task_pose()¶
Returns the Cartesian pose of the end effector with respect the base frame.
- Returns
list – The position of the end-effector as a list with 3 elements
Quaternion – The orientation of the end-effector as quaternion
- reset_joint_position(joint_position)¶
Resets the joint positions of the robot by teleporting it. This is of course for a simulated robot, not a real one :). For real robots just call set_joint_trajectory to smoothly move the root.
- Parameters
joint_position (list) – A list of floats in rad for the commanded joint positions of the robot.
- reset_task_pose(pos, quat)¶
Resets/Teleports the robot to the desired pose. This is of course for a simulated robot, not a real one :). For real robots just call set_task_pose_trajectory to smoothly move the robot.
- Parameters
pos (list) – The desired position of the end-effector as a list with 3 elements
quat (Quaternion) – The desired orientation of the end-effector as quaternion
- set_joint_position(joint_position)¶
Commands the robot with desired joint positions.
- Parameters
joint_position (list) – A list of floats in rad for the commanded joint positions of the robot.
- set_joint_trajectory(final, duration)¶
Commands the robot with a desired joint configuration be reached with 5th order spline. The reaching is performed in a straight line.
- Parameters
joint (list) – The joint configuration to be reached in rad.
duration (float) – The duration of the motion
- set_task_pose(pos, quat)¶
Commands the robot with a desired cartesian pose. This should solve the inverse kinematics of the robot for the desired pose and call set_joint_position. Returns the Cartesian pose of the end effector with respect the base frame.
- Parameters
pos (list) – The position of the end-effector as a list with 3 elements
quat (Quaternion) – The orientation of the end-effector as quaternion
- clt_core.env.empty_push(obs, next_obs, eps=0.005)¶
Checks if the objects have been moved
- clt_core.env.get_heightmap(obs)¶
Computes the heightmap based on the ‘depth’ and ‘seg’. In this heightmap table pixels has value zero, objects > 0 and everything below the table <0.
- Parameters
obs (dict) – The dictionary with the visual and full state of the environment.
- Returns
Array with the heightmap.
- Return type
np.ndarray