def __init__(self):
self.time_step = 0.004
self.finger = sim_finger.SimFinger(finger_type=Cage.args.finger_type,
time_step=self.time_step,
enable_visualization=True,)
self.num_fingers = self.finger.number_of_fingers
_kwargs = {"physicsClientId": self.finger._pybullet_client_id}
if Cage.args.control_mode == "position":
self.position_goals = visual_objects.Marker(number_of_goals=self.num_fingers)
self.initial_robot_position=np.array([0.0, np.deg2rad(-60), np.deg2rad(-60)] * 3, dtype=np.float32,)
self.finger.reset_finger_positions_and_velocities(self.initial_robot_position)
self.initial_object_pose = move_cube.Pose(position=[0, 0, move_cube._min_height],
orientation=move_cube.Pose().orientation)
self.cube = collision_objects.Block(self.initial_object_pose.position,
self.initial_object_pose.orientation, mass=0.020, **_kwargs)
self.cube_z = 0.0325
self.workspace_radius = 0.18
self.tricamera = camera.TriFingerCameras(**_kwargs)
self.initial_marker_position = np.array([[ 0. , 0.17856407, self.cube_z],
[ 0.15464102, -0.08928203, self.cube_z],
[-0.15464102, -0.08928203, self.cube_z]])
self.position_goals.set_state(self.initial_marker_position)
self.marker_position = self.initial_marker_position
self.ik_module = geometric_ik.GeometricIK()
def _create_dummy_goal_object(self, obs):
dummy_cube = collision_objects.Block(
obs['goal_object_position'],
obs['object_orientation'],
mass=0.020,
)
return dummy_cube
def execute_random_motion(*, finger_name, nb_timesteps, enable_visualization):
finger = SimFinger(
finger_type=finger_name,
time_step=0.004,
enable_visualization=enable_visualization,
)
cube = collision_objects.Block()
for t in range(nb_timesteps):
if t % 50 == 0:
torque = np.random.uniform(low=-0.36, high=0.36, size=(9))
finger.append_desired_action(finger.Action(torque=torque))
del finger
del cube
def __init__(
self,
visualization=False,
initial_robot_position=None,
initial_object_pose=None,
enable_cameras=False,
):
"""Initialize.
Args:
visualization (bool): Set to true to run visualization.
initial_robot_position: Initial robot joint angles
initial_object_pose: Initial pose for the manipulation object.
Can be any object with attributes ``position`` (x, y, z) and
``orientation`` (x, y, z, w). This is optional, if not set, a
random pose will be sampled.
enable_cameras (bool): Set to true to enable camera observations.
By default this is disabled as rendering of images takes a lot
of computational power. Therefore the cameras should only be
enabled if the images are actually used.
"""
#: Camera rate in frames per second. Observations of camera and
#: object pose will only be updated with this rate.
#: NOTE: This is currently not used!
self._camera_rate_fps = 30
#: Set to true to render camera observations
self.enable_cameras = enable_cameras
#: Simulation time step
self._time_step = 0.004
# first camera update in the first step
self._next_camera_update_step = 0
# Initialize robot, object and cameras
# ====================================
self.simfinger = SimFinger(
finger_type="trifingerpro",
time_step=self._time_step,
enable_visualization=visualization,
)
if initial_robot_position is None:
initial_robot_position = self.spaces.robot_position.default
self.simfinger.reset_finger_positions_and_velocities(
initial_robot_position)
if initial_object_pose is None:
initial_object_pose = move_cube.Pose(
position=self.spaces.object_position.default,
orientation=self.spaces.object_orientation.default,
)
self.cube = collision_objects.Block(
initial_object_pose.position,
initial_object_pose.orientation,
mass=0.020,
)
self.tricamera = camera.TriFingerCameras()
# Forward some methods for convenience
# ====================================
# forward "RobotFrontend" methods directly to simfinger
self.Action = self.simfinger.Action
self.get_desired_action = self.simfinger.get_desired_action
self.get_applied_action = self.simfinger.get_applied_action
self.get_timestamp_ms = self.simfinger.get_timestamp_ms
self.get_current_timeindex = self.simfinger.get_current_timeindex
self.get_robot_observation = self.simfinger.get_observation
# forward kinematics directly to simfinger
self.forward_kinematics = (
self.simfinger.pinocchio_utils.forward_kinematics)
# Initialize log
# ==============
self._action_log = {
"initial_robot_position": initial_robot_position.tolist(),
"initial_object_pose": {
"position": initial_object_pose.position.tolist(),
"orientation": initial_object_pose.orientation.tolist(),
},
"actions": [],
}
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--finger-type",
choices=["single", "tri"],
required=True,
help="""Specify whether the Single Finger ("single") or the TriFinger
("tri") is used.
""",
)
parser.add_argument(
"--real-time-mode",
"-r",
action="store_true",
help="""Run simulation in real time. If not set, the simulation runs
as fast as possible.
""",
)
parser.add_argument(
"--first-action-timeout",
"-t",
type=float,
default=math.inf,
help="""Timeout (in seconds) for reception of first action after
starting the backend. If not set, the timeout is disabled.
""",
)
parser.add_argument(
"--max-number-of-actions",
"-a",
type=int,
default=0,
help="""Maximum numbers of actions that are processed. After this the
backend shuts down automatically.
""",
)
parser.add_argument(
"--logfile",
"-l",
type=str,
help="""Path to a file to which the data log is written. If not
specified, no log is generated.
""",
)
parser.add_argument(
"--add-cube",
action="store_true",
help="""Spawn a cube and run the object tracker backend.""",
)
parser.add_argument(
"--cameras",
action="store_true",
help="""Run camera backend using rendered images.""",
)
parser.add_argument(
"--visualize",
"-v",
action="store_true",
help="Run pyBullet's GUI for visualization.",
)
args = parser.parse_args()
# select the correct types/functions based on which robot is used
if args.finger_type == "single":
shared_memory_id = "finger"
finger_types = robot_interfaces.finger
create_backend = drivers.create_single_finger_backend
else:
shared_memory_id = "trifinger"
finger_types = robot_interfaces.trifinger
create_backend = drivers.create_trifinger_backend
robot_data = finger_types.MultiProcessData(shared_memory_id, True)
if args.logfile:
logger = finger_types.Logger(robot_data, 100)
logger.start(args.logfile)
backend = create_backend(
robot_data,
args.real_time_mode,
args.visualize,
args.first_action_timeout,
args.max_number_of_actions,
)
backend.initialize()
# Object and Object Tracker Interface
# Important: These objects need to be created _after_ the simulation is
# initialized (i.e. after the SimFinger instance is created).
if args.add_cube:
# only import when really needed
import trifinger_object_tracking.py_object_tracker as object_tracker
# spawn a cube in the arena
cube = collision_objects.Block()
# initialize the object tracker interface
object_tracker_data = object_tracker.Data("object_tracker", True)
object_tracker_backend = object_tracker.SimulationBackend(
object_tracker_data, cube, args.real_time_mode
)
if args.cameras:
from trifinger_cameras import tricamera
camera_data = tricamera.MultiProcessData("tricamera", True, 10)
camera_driver = tricamera.PyBulletTriCameraDriver()
camera_backend = tricamera.Backend(camera_driver, camera_data)
backend.wait_until_terminated()
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--save-poses",
type=str,
metavar="FILENAME",
help="""If set, the demo stops when the history of the object pose time
series is full and stores the buffered poses to the specified file.
""",
)
parser.add_argument(
"--history-size",
type=int,
default=1000,
help="""Size of the object pose time series. Default: %(default)d""",
)
parser.add_argument(
"--non-real-time",
action="store_true",
help="""Do not run the simulation in real-time but as fast as
possible.
""",
)
args = parser.parse_args()
real_time = not args.non_real_time
time_step = 0.004
finger = sim_finger.SimFinger(
finger_type="trifingerone",
time_step=time_step,
enable_visualization=True,
)
# Object and Object Tracker Interface
# ===================================
#
# Important: These objects need to be created _after_ the simulation is
# initialized (i.e. after the SimFinger instance is created).
# spawn a cube in the arena
cube = collision_objects.Block()
# initialize the object tracker interface
object_tracker_data = object_tracker.Data("object_tracker", True,
args.history_size)
object_tracker_backend = object_tracker.SimulationBackend(
object_tracker_data, cube, real_time)
object_tracker_frontend = object_tracker.Frontend(object_tracker_data)
# Move the fingers to random positions so that the cube is kicked around
# (and thus it's position changes).
while True:
goal = np.array(
sample.random_joint_positions(
number_of_fingers=3,
lower_bounds=[-1, -1, -2],
upper_bounds=[1, 1, 2],
))
finger_action = finger.Action(position=goal)
for _ in range(50):
t = finger.append_desired_action(finger_action)
finger.get_observation(t)
if real_time:
time.sleep(time_step)
# get the latest pose estimate of the cube and print it
t_obj = object_tracker_frontend.get_current_timeindex()
cube_pose = object_tracker_frontend.get_pose(t_obj)
print("Cube Pose: %s" % cube_pose)
# if --save-poses is set, stop when the buffer is full and write it to
# a file
if args.save_poses and t_obj > args.history_size:
# the backend needs to be stopped before saving the buffered poses
object_tracker_backend.stop()
object_tracker_backend.store_buffered_data(args.save_poses)
break