import yaml
from gibson2.core.physics.robot_locomotors import Turtlebot, Husky, Ant, Humanoid, JR2, JR2_Kinova
from gibson2.core.simulator import Simulator
from gibson2.core.physics.scene import BuildingScene, StadiumScene
from gibson2.utils.utils import parse_config
from gibson2.core.physics.interactive_objects import InteractiveObj
import gibson2
import os
import numpy as np
import pybullet as p
if __name__ == '__main__':
s = Simulator(mode='gui')
scene = BuildingScene('Ohoopee')
s.import_scene(scene)
door = InteractiveObj(os.path.join(gibson2.assets_path, 'models',
'scene_components', 'realdoor.urdf'),
scale=0.3)
s.import_interactive_object(door)
x = p.addUserDebugParameter('x', -10, 10, 0)
y = p.addUserDebugParameter('y', -10, 10, 0)
z = p.addUserDebugParameter('z', -5, 5, 0)
rotate = p.addUserDebugParameter('rotate', -np.pi, np.pi, 0)
while True:
x_pos = p.readUserDebugParameter(x)
y_pos = p.readUserDebugParameter(y)
z_pos = p.readUserDebugParameter(z)
rotate_pos = p.readUserDebugParameter(rotate)
import yaml
import math
import trimesh
from pyquaternion import Quaternion
from transformations import euler_from_matrix
import pybullet as pb
from gibson2.core.simulator import Simulator
from gibson2.core.physics.interactive_objects import InteractiveObj
import matplotlib.pyplot as plt
'''
Analyzes a model for heights of surfaces within it.
Assumes model is a cabinet or otherwise has sets of shelves within it, so just need to
find the discrete set of heights the shelves are at.
'''
simulator = Simulator(image_width=640, image_height=640)
parser = argparse.ArgumentParser(
description='Finds heights of shelves in a container object.')
parser.add_argument('object_file', type=str)
args = parser.parse_args()
out_dict = {}
obj = InteractiveObj(filename=args.object_file, scale=1.0)
body_id = simulator.import_object(obj)
aabb = pb.getAABB(body_id)
size = [
aabb[1][0] - aabb[0][0], aabb[1][1] - aabb[0][1], aabb[1][2] - aabb[0][2]
]
urdf_dir = os.path.dirname(args.object_file)
with open(args.object_file, 'r') as f:
def main():
config = parse_config('../configs/fetch_p2p_nav.yaml')
s = Simulator(mode='gui', timestep=1 / 240.0)
scene = EmptyScene()
s.import_scene(scene)
fetch = Fetch(config)
s.import_robot(fetch)
robot_id = fetch.robot_ids[0]
arm_joints = joints_from_names(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'
])
#finger_joints = joints_from_names(robot_id, ['l_gripper_finger_joint', 'r_gripper_finger_joint'])
fetch.robot_body.reset_position([0, 0, 0])
fetch.robot_body.reset_orientation([0, 0, 1, 0])
x, y, z = fetch.get_end_effector_position()
#set_joint_positions(robot_id, finger_joints, [0.04,0.04])
print(x, y, z)
visual_marker = p.createVisualShape(p.GEOM_SPHERE, radius=0.02)
marker = p.createMultiBody(baseVisualShapeIndex=visual_marker)
#max_limits = [0,0] + get_max_limits(robot_id, arm_joints) + [0.05,0.05]
#min_limits = [0,0] + get_min_limits(robot_id, arm_joints) + [0,0]
#rest_position = [0,0] + list(get_joint_positions(robot_id, arm_joints)) + [0.04,0.04]
max_limits = [0, 0] + get_max_limits(robot_id, arm_joints)
min_limits = [0, 0] + get_min_limits(robot_id, arm_joints)
rest_position = [0, 0] + list(get_joint_positions(robot_id, arm_joints))
joint_range = list(np.array(max_limits) - np.array(min_limits))
joint_range = [item + 1 for item in joint_range]
jd = [0.1 for item in joint_range]
print(max_limits)
print(min_limits)
def accurateCalculateInverseKinematics(robotid, endEffectorId, targetPos,
threshold, maxIter):
sample_fn = get_sample_fn(robotid, arm_joints)
set_joint_positions(robotid, arm_joints, sample_fn())
it = 0
while it < maxIter:
jointPoses = p.calculateInverseKinematics(robotid,
endEffectorId,
targetPos,
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=joint_range,
restPoses=rest_position,
jointDamping=jd)
set_joint_positions(robotid, arm_joints, jointPoses[2:10])
ls = p.getLinkState(robotid, endEffectorId)
newPos = ls[4]
dist = np.linalg.norm(np.array(targetPos) - np.array(newPos))
if dist < threshold:
break
it += 1
print("Num iter: " + str(it) + ", threshold: " + str(dist))
return jointPoses
while True:
with Profiler("Simulation step"):
fetch.robot_body.reset_position([0, 0, 0])
fetch.robot_body.reset_orientation([0, 0, 1, 0])
threshold = 0.01
maxIter = 100
joint_pos = accurateCalculateInverseKinematics(
robot_id, fetch.parts['gripper_link'].body_part_index,
[x, y, z], threshold, maxIter)[2:10]
#set_joint_positions(robot_id, finger_joints, [0.04, 0.04])
s.step()
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_IS_DOWN)):
x += 0.01
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_IS_DOWN)):
x -= 0.01
if (k == p.B3G_UP_ARROW and (v & p.KEY_IS_DOWN)):
y += 0.01
if (k == p.B3G_DOWN_ARROW and (v & p.KEY_IS_DOWN)):
y -= 0.01
if (k == ord('z') and (v & p.KEY_IS_DOWN)):
z += 0.01
if (k == ord('x') and (v & p.KEY_IS_DOWN)):
z -= 0.01
p.resetBasePositionAndOrientation(marker, [x, y, z], [0, 0, 0, 1])
s.disconnect()
def test_import_building_big():
s = Simulator(mode='headless')
scene = BuildingScene('Ohoopee')
s.import_scene(scene, texture_scale=1)
assert s.objects == list(range(2))
s.disconnect()
import yaml
from gibson2.core.physics.robot_locomotors import Turtlebot, JR2_Kinova, Fetch
from gibson2.core.simulator import Simulator
from gibson2.core.physics.scene import EmptyScene
from gibson2.core.physics.interactive_objects import InteractiveObj, BoxShape, YCBObject
from gibson2.utils.utils import parse_config
from gibson2.core.render.profiler import Profiler
import pytest
import pybullet as p
import numpy as np
config = parse_config('../configs/jr_interactive_nav.yaml')
s = Simulator(mode='headless', timestep=1 / 240.0)
scene = EmptyScene()
s.import_scene(scene)
jr = JR2_Kinova(config)
s.import_robot(jr)
jr.robot_body.reset_position([0, 0, 0])
jr.robot_body.reset_orientation([0, 0, 1, 0])
fetch = Fetch(config)
s.import_robot(fetch)
fetch.robot_body.reset_position([0, 1, 0])
fetch.robot_body.reset_orientation([0, 0, 1, 0])
obj = InteractiveObj(
filename='/data4/mdv0/cabinet/0007/part_objs/cabinet_0007.urdf')
s.import_interactive_object(obj)
obj.set_position([-2, 0, 0.5])
obj = InteractiveObj(
filename='/data4/mdv0/cabinet/0007/part_objs/cabinet_0007.urdf')
s.import_interactive_object(obj)