def main():
time_step = 0.004
finger = sim_finger.SimFinger(
finger_type="trifingerone",
time_step=time_step,
enable_visualization=True,
)
# Important: The cameras need the be created _after_ the simulation is
# initialized.
cameras = camera.TriFingerCameras()
# Move the fingers to random positions
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)
images = cameras.get_images()
# images are rgb --> convert to bgr for opencv
images = [cv2.cvtColor(img, cv2.COLOR_RGB2BGR) for img in images]
cv2.imshow("camera60", images[0])
cv2.imshow("camera180", images[1])
cv2.imshow("camera300", images[2])
cv2.waitKey(int(time_step * 1000))
def main():
# argparser = argparse.ArgumentParser(description=__doc__)
# args = argparser.parse_args()
time_step = 0.001
finger = sim_finger.SimFinger(
finger_type="trifingerpro",
time_step=time_step,
enable_visualization=True,
)
markers = []
while True:
# action = finger.Action(torque=[0.3, 0.3, -0.2] * 3)
action = finger.Action(position=[0.0, 0.9, -1.7] * 3)
t = finger.append_desired_action(action)
finger.get_observation(t)
# delete old markers
for m in markers:
del m
markers = visualize_collisions(finger)
time.sleep(time_step)
def main():
argparser = argparse.ArgumentParser(description=__doc__)
argparser.add_argument(
"--control-mode",
default="position",
choices=["position", "torque"],
help="Specify position or torque as the control mode.",
)
argparser.add_argument(
"--finger-type",
default="trifingerone",
choices=finger_types_data.get_valid_finger_types(),
help="Specify a valid finger type",
)
args = argparser.parse_args()
time_step = 0.004
finger = sim_finger.SimFinger(
finger_type=args.finger_type,
time_step=time_step,
enable_visualization=True,
)
num_fingers = finger.number_of_fingers
if args.control_mode == "position":
position_goals = visual_objects.Marker(number_of_goals=num_fingers)
while True:
if args.control_mode == "position":
desired_joint_positions = np.array(
sample.random_joint_positions(number_of_fingers=num_fingers))
finger_action = finger.Action(position=desired_joint_positions)
# visualize the goal position of the finger tip
position_goals.set_state(
finger.pinocchio_utils.forward_kinematics(
desired_joint_positions))
if args.control_mode == "torque":
desired_joint_torques = [random.random()] * 3 * num_fingers
finger_action = finger.Action(torque=desired_joint_torques)
# pursue this goal for one second
for _ in range(int(1 / time_step)):
t = finger.append_desired_action(finger_action)
finger.get_observation(t)
time.sleep(time_step)
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_visualizartion=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
import matplotlib.pyplot as plt
from trifinger_simulation import sim_finger, sample
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--time-step", "-t", type=float, required=True)
parser.add_argument("--plot", action="store_true")
args = parser.parse_args()
time_step = args.time_step
finger = sim_finger.SimFinger(
time_step=time_step,
enable_visualization=True,
finger_type="fingerone",
)
# set the finger to a reasonable start position
finger.reset_finger_positions_and_velocities([0, -0.7, -1.5])
errors = []
for _ in range(100):
target_position = np.array(
sample.random_joint_positions(number_of_fingers=1)
)
action = finger.Action(position=target_position)
positions = []
for _ in range(500):
t = finger.append_desired_action(action)
from trifinger_simulation import sim_finger, sample
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--time-step", "-t", type=float, required=True)
parser.add_argument("--plot", type=int)
args = parser.parse_args()
time_step = args.time_step
finger = sim_finger.SimFinger(
time_step=time_step,
enable_visualization=True,
finger_type="fingerpro",
# spawn robot higher up to avoid collisions with the table
robot_position_offset=(0, 0, 0.5),
)
errors = []
for _ in range(100):
target_position = np.array(
sample.random_joint_positions(
number_of_fingers=1,
lower_bounds=[-0.33, 0.0, -2.7],
upper_bounds=[1.0, 1.57, 0.0],
)
)
action = finger.Action(position=target_position)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("log_dir",
type=pathlib.Path,
help="Path to the log files.")
parser.add_argument("--rate",
type=int,
default=1,
help="Time in ms per step.")
args = parser.parse_args()
robot_log_file = str(args.log_dir / "robot_data.dat")
camera_log_file = str(args.log_dir / "camera_data.dat")
log = robot_fingers.TriFingerPlatformLog(robot_log_file, camera_log_file)
simulation = sim_finger.SimFinger(
finger_type="trifingerpro",
enable_visualization=True,
)
cameras = camera.create_trifinger_camera_array_from_config(args.log_dir)
cube_urdf_file = (pathlib.Path(trifinger_simulation.__file__).parent /
"data/cube_v2/cube_v2.urdf")
cube = pybullet.loadURDF(fileName=str(cube_urdf_file), )
assert cube >= 0, "Failed to load cube model."
pybullet.configureDebugVisualizer(lightPosition=(0, 0, 1.0))
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SHADOWS, 1)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_RGB_BUFFER_PREVIEW,
0)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_DEPTH_BUFFER_PREVIEW,
0)
# yes, it is really a segmentation maRk...
pybullet.configureDebugVisualizer(
pybullet.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
for t in range(log.get_first_timeindex(),
log.get_last_timeindex() + 1, 100):
robot_observation = log.get_robot_observation(t)
simulation.reset_finger_positions_and_velocities(
robot_observation.position)
# get rendered images from simulation
sim_images = cameras.get_images()
# images are rgb --> convert to bgr for opencv
sim_images = [
cv2.cvtColor(img, cv2.COLOR_RGB2BGR) for img in sim_images
]
sim_images = np.hstack(sim_images)
# get real images from cameras
try:
camera_observation = log.get_camera_observation(t)
# set cube pose
pybullet.resetBasePositionAndOrientation(
cube,
camera_observation.object_pose.position,
camera_observation.object_pose.orientation,
)
real_images = [
utils.convert_image(cam.image)
for cam in camera_observation.cameras
]
real_images = np.hstack(real_images)
except Exception as e:
print(e)
real_images = np.zeros_like(sim_images)
# display images
image = np.vstack((sim_images, real_images))
cv2.imshow("cameras", image)
key = cv2.waitKey(args.rate)
# exit on "q"
if key == ord("q"):
return
sim_finger,
)
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument(
"finger_type",
choices=finger_types_data.get_valid_finger_types(),
)
args = parser.parse_args()
time_step = 0.001
finger = sim_finger.SimFinger(
finger_type=args.finger_type,
time_step=time_step,
enable_visualization=True,
)
_config: typing.Dict[str, typing.Dict[str, typing.Sequence[float]]] = {
"fingerone": {
"joint_pos": (0, -0.7, -1.5),
"cube_pos": (0.05, -0.08, 0.04),
},
"trifingerone": {
"joint_pos": (0, -0.7, -1.5) * 3,
"cube_pos": (0.05, 0.08, 0.04),
},
"fingeredu": {
"joint_pos": (0, 0.7, -1.5),
"cube_pos": (0.08, 0.05, 0.04),
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--robot",
"-r",
choices=["trifingerone", "trifingerpro"],
default="trifingerone",
help="Which robot to use. Default: %(default)s",
)
parser.add_argument(
"--config-dir",
"-c",
type=pathlib.Path,
help="""Path to the directory containing camera calibration files. This
is optional, if not specified, some default values will be used.
""",
)
parser.add_argument(
"--param-file-format",
type=str,
default="camera{id}.yml",
help="""Format of the camera parameter files. '{id}' is replaced with
the camera id. Default: %(default)s
""",
)
args = parser.parse_args()
time_step = 0.004
finger = sim_finger.SimFinger(
finger_type=args.robot,
time_step=time_step,
enable_visualization=True,
)
# Important: The cameras need the be created _after_ the simulation is
# initialized.
if args.config_dir:
cameras = camera.create_trifinger_camera_array_from_config(
args.config_dir, calib_filename_pattern=args.param_file_format)
else:
cameras = camera.TriFingerCameras()
# Move the fingers to random positions
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)
images = cameras.get_images()
# images are rgb --> convert to bgr for opencv
images = [cv2.cvtColor(img, cv2.COLOR_RGB2BGR) for img in images]
cv2.imshow("camera60", images[0])
cv2.imshow("camera180", images[1])
cv2.imshow("camera300", images[2])
key = cv2.waitKey(int(time_step * 1000))
if key == ord("q"):
return