Merge branch 'release-cleanup' into physics_render
This commit is contained in:
Fei Xia
GitHub
commit
fcd74713d9
|
|
@ -0,0 +1,83 @@
|
|||
# scene
|
||||
scene: building
|
||||
#model_id: Kerrtown
|
||||
model_id: Norvelt
|
||||
#model_id: Albertville
|
||||
build_graph: true
|
||||
load_texture: true
|
||||
should_load_replaced_objects: false
|
||||
should_load_additional_objects: false
|
||||
trav_map_erosion: 3
|
||||
|
||||
# robot
|
||||
robot: Fetch
|
||||
|
||||
action_high: 0.1
|
||||
action_low: -0.05
|
||||
|
||||
# task, observation and action
|
||||
task: pointgoal # pointgoal|objectgoal|areagoal|reaching
|
||||
fisheye: false
|
||||
|
||||
initial_pos: [0, 0, 0.0]
|
||||
initial_orn: [0.0, 0.0, 0.0]
|
||||
|
||||
target_pos: [0, 2, 0.0]
|
||||
target_orn: [0.0, 0.0, 0.0]
|
||||
|
||||
is_discrete: false
|
||||
additional_states_dim: 25
|
||||
|
||||
# reward
|
||||
reward_type: dense
|
||||
success_reward: 10.0
|
||||
slack_reward: 0.0
|
||||
potential_reward_weight: 1.0
|
||||
electricity_reward_weight: 0.0
|
||||
stall_torque_reward_weight: 0.0
|
||||
collision_reward_weight: -0.0
|
||||
collision_ignore_link_a_ids: [0, 1, 2] # ignore collision with these agent's link ids
|
||||
|
||||
# discount factor
|
||||
discount_factor: 0.99
|
||||
|
||||
# termination condition
|
||||
death_z_thresh: 0.2
|
||||
dist_tol: 0.5 # body width
|
||||
max_step: 500
|
||||
|
||||
# sensor
|
||||
output: [sensor, depth, scan]
|
||||
resolution: 256
|
||||
fov: 54
|
||||
n_horizontal_rays: 220
|
||||
n_vertical_beams: 1
|
||||
|
||||
# display
|
||||
use_filler: true
|
||||
display_ui: false
|
||||
show_diagnostics: false
|
||||
ui_num: 2
|
||||
ui_components: [RGB_FILLED, DEPTH]
|
||||
random:
|
||||
random_initial_pose: false
|
||||
random_target_pose: false
|
||||
random_init_x_range: [-0.1, 0.1]
|
||||
random_init_y_range: [-0.1, 0.1]
|
||||
random_init_z_range: [-0.1, 0.1]
|
||||
random_init_rot_range: [-0.1, 0.1]
|
||||
|
||||
speed:
|
||||
timestep: 0.001
|
||||
frameskip: 10
|
||||
|
||||
mode: web_ui #gui|headless
|
||||
verbose: false
|
||||
fast_lq_render: true
|
||||
|
||||
# visual objects
|
||||
visual_object_at_initial_target_pos: true
|
||||
target_visual_object_visible_to_agent: false
|
||||
|
||||
# debug
|
||||
debug: false
|
||||
|
|
@ -0,0 +1,82 @@
|
|||
# scene
|
||||
scene: building
|
||||
model_id: candcenter
|
||||
build_graph: true
|
||||
load_texture: true
|
||||
should_load_replaced_objects: false
|
||||
should_load_additional_objects: false
|
||||
trav_map_erosion: 3
|
||||
|
||||
# robot
|
||||
robot: Fetch
|
||||
velocity: 0.1
|
||||
fine_motion_plan: true
|
||||
|
||||
# task, observation and action
|
||||
task: pointgoal # pointgoal|objectgoal|areagoal|reaching
|
||||
fisheye: false
|
||||
|
||||
initial_pos: [-1.0, -1.0, 0.0]
|
||||
initial_orn: [0.0, 0.0, 0.0]
|
||||
|
||||
target_pos: [1.0, 1.0, 0.0]
|
||||
target_orn: [0.0, 0.0, 0.0]
|
||||
|
||||
is_discrete: false
|
||||
additional_states_dim: 22
|
||||
|
||||
# reward
|
||||
reward_type: dense
|
||||
success_reward: 10.0
|
||||
slack_reward: 0.0
|
||||
potential_reward_weight: 1.0
|
||||
electricity_reward_weight: 0.0
|
||||
stall_torque_reward_weight: 0.0
|
||||
collision_reward_weight: -0.0
|
||||
collision_ignore_link_a_ids: [0, 1, 2, 3, 20, 21, 22] # ignore collision with these agent's link ids
|
||||
|
||||
# discount factor
|
||||
discount_factor: 0.99
|
||||
|
||||
# termination condition
|
||||
death_z_thresh: 0.2
|
||||
dist_tol: 0.5 # body width
|
||||
max_step: 25
|
||||
|
||||
# sensor
|
||||
output: [sensor, rgb, depth, scan]
|
||||
resolution: 128
|
||||
fov: 57.5
|
||||
n_horizontal_rays: 220
|
||||
n_vertical_beams: 1
|
||||
scan_noise_rate: 0.02
|
||||
depth_noise_rate: 0.02
|
||||
|
||||
# display
|
||||
use_filler: true
|
||||
display_ui: false
|
||||
show_diagnostics: false
|
||||
ui_num: 2
|
||||
ui_components: [RGB_FILLED, DEPTH]
|
||||
random:
|
||||
random_initial_pose: false
|
||||
random_target_pose: false
|
||||
random_init_x_range: [-0.1, 0.1]
|
||||
random_init_y_range: [-0.1, 0.1]
|
||||
random_init_z_range: [-0.1, 0.1]
|
||||
random_init_rot_range: [-0.1, 0.1]
|
||||
|
||||
speed:
|
||||
timestep: 0.001
|
||||
frameskip: 10
|
||||
|
||||
mode: web_ui #gui|headless
|
||||
verbose: false
|
||||
fast_lq_render: true
|
||||
|
||||
# visual objects
|
||||
visual_object_at_initial_target_pos: true
|
||||
target_visual_object_visible_to_agent: false
|
||||
|
||||
# debug
|
||||
debug: false
|
||||
|
|
@ -0,0 +1,83 @@
|
|||
# scene
|
||||
scene: building
|
||||
model_id: candcenter
|
||||
build_graph: true
|
||||
load_texture: true
|
||||
should_load_replaced_objects: false
|
||||
should_load_additional_objects: false
|
||||
trav_map_erosion: 3
|
||||
|
||||
# robot
|
||||
robot: Fetch
|
||||
wheel_velocity: 0.05
|
||||
torso_lift_velocity: 0.0001
|
||||
arm_velocity: 0.01
|
||||
|
||||
# task, observation and action
|
||||
task: pointgoal # pointgoal|objectgoal|areagoal|reaching
|
||||
fisheye: false
|
||||
|
||||
initial_pos: [-1.0, -1.0, 0.0]
|
||||
initial_orn: [0.0, 0.0, 0.0]
|
||||
|
||||
target_pos: [1.0, 1.0, 0.0]
|
||||
target_orn: [0.0, 0.0, 0.0]
|
||||
|
||||
is_discrete: false
|
||||
additional_states_dim: 22
|
||||
|
||||
# reward
|
||||
reward_type: dense
|
||||
success_reward: 10.0
|
||||
slack_reward: 0.0
|
||||
potential_reward_weight: 1.0
|
||||
electricity_reward_weight: 0.0
|
||||
stall_torque_reward_weight: 0.0
|
||||
collision_reward_weight: -0.0
|
||||
collision_ignore_link_a_ids: [0, 1, 2, 3, 20, 21, 22] # ignore collision with these agent's link ids
|
||||
|
||||
# discount factor
|
||||
discount_factor: 0.99
|
||||
|
||||
# termination condition
|
||||
death_z_thresh: 0.2
|
||||
dist_tol: 0.5 # body width
|
||||
max_step: 750
|
||||
|
||||
# sensor
|
||||
output: [sensor, rgb, depth, scan]
|
||||
resolution: 128
|
||||
fov: 57.5
|
||||
n_horizontal_rays: 220
|
||||
n_vertical_beams: 1
|
||||
scan_noise_rate: 0.02
|
||||
depth_noise_rate: 0.02
|
||||
|
||||
# display
|
||||
use_filler: true
|
||||
display_ui: false
|
||||
show_diagnostics: false
|
||||
ui_num: 2
|
||||
ui_components: [RGB_FILLED, DEPTH]
|
||||
random:
|
||||
random_initial_pose: false
|
||||
random_target_pose: false
|
||||
random_init_x_range: [-0.1, 0.1]
|
||||
random_init_y_range: [-0.1, 0.1]
|
||||
random_init_z_range: [-0.1, 0.1]
|
||||
random_init_rot_range: [-0.1, 0.1]
|
||||
|
||||
speed:
|
||||
timestep: 0.001
|
||||
frameskip: 10
|
||||
|
||||
mode: web_ui #gui|headless
|
||||
verbose: false
|
||||
fast_lq_render: true
|
||||
|
||||
# visual objects
|
||||
visual_object_at_initial_target_pos: true
|
||||
target_visual_object_visible_to_agent: false
|
||||
|
||||
# debug
|
||||
debug: false
|
||||
|
|
@ -47,7 +47,7 @@ dist_tol: 0.333 # body width
|
|||
max_step: 500
|
||||
|
||||
# sensor
|
||||
output: [sensor, depth_seg]
|
||||
output: [sensor, depth]
|
||||
resolution: 68
|
||||
fov: 120
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,85 @@
|
|||
# scene
|
||||
scene: building
|
||||
#model_id: Kerrtown
|
||||
model_id: area1
|
||||
#model_id: Albertville
|
||||
build_graph: true
|
||||
load_texture: true
|
||||
should_load_replaced_objects: false
|
||||
should_load_additional_objects: true
|
||||
trav_map_erosion: 2
|
||||
|
||||
# robot
|
||||
robot: Turtlebot
|
||||
|
||||
action_high: 0.1
|
||||
action_low: -0.05
|
||||
|
||||
# task, observation and action
|
||||
task: pointgoal # pointgoal|objectgoal|areagoal|reaching
|
||||
fisheye: false
|
||||
|
||||
initial_pos: [0, 0, 0.0]
|
||||
initial_orn: [0.0, 0.0, 0.0]
|
||||
|
||||
target_pos: [0, 2, 0.0]
|
||||
target_orn: [0.0, 0.0, 0.0]
|
||||
|
||||
is_discrete: false
|
||||
additional_states_dim: 25
|
||||
|
||||
# reward
|
||||
reward_type: dense
|
||||
success_reward: 10.0
|
||||
slack_reward: 0.0
|
||||
potential_reward_weight: 1.0
|
||||
electricity_reward_weight: 0.0
|
||||
stall_torque_reward_weight: 0.0
|
||||
collision_reward_weight: -0.0
|
||||
collision_ignore_link_a_ids: [1, 2, 3, 4] # ignore collision with these agent's link ids
|
||||
|
||||
# discount factor
|
||||
discount_factor: 0.99
|
||||
|
||||
# termination condition
|
||||
death_z_thresh: 0.2
|
||||
dist_tol: 0.333 # body width
|
||||
max_step: 500
|
||||
|
||||
# sensor
|
||||
output: [sensor, rgbd, scan, seg_pred]
|
||||
resolution: 96
|
||||
#output: [sensor, rgbd, seg_pred]
|
||||
#resolution: 320
|
||||
fov: 58
|
||||
n_horizontal_rays: 228
|
||||
n_vertical_beams: 1
|
||||
|
||||
# display
|
||||
use_filler: true
|
||||
display_ui: false
|
||||
show_diagnostics: false
|
||||
ui_num: 2
|
||||
ui_components: [RGB_FILLED, DEPTH]
|
||||
random:
|
||||
random_initial_pose: false
|
||||
random_target_pose: false
|
||||
random_init_x_range: [-0.1, 0.1]
|
||||
random_init_y_range: [-0.1, 0.1]
|
||||
random_init_z_range: [-0.1, 0.1]
|
||||
random_init_rot_range: [-0.1, 0.1]
|
||||
|
||||
speed:
|
||||
timestep: 0.001
|
||||
frameskip: 10
|
||||
|
||||
mode: web_ui #gui|headless
|
||||
verbose: false
|
||||
fast_lq_render: true
|
||||
|
||||
# visual objects
|
||||
visual_object_at_initial_target_pos: true
|
||||
target_visual_object_visible_to_agent: false
|
||||
|
||||
# debug
|
||||
debug: false
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
# scene
|
||||
scene: building
|
||||
model_id: Ohoopee
|
||||
model_id: Avonia
|
||||
|
||||
# robot
|
||||
robot: Turtlebot
|
||||
|
|
@ -37,9 +37,9 @@ dist_tol: 0.2
|
|||
max_step: 500
|
||||
|
||||
# sensor
|
||||
output: [sensor, rgb, depth]
|
||||
resolution: 256
|
||||
fov: 90
|
||||
output: [sensor, rgb, depth, pc]
|
||||
resolution: 128
|
||||
fov: 120
|
||||
|
||||
# display
|
||||
use_filler: true
|
||||
|
|
|
|||
|
|
@ -9,35 +9,52 @@ from gibson2.core.render.profiler import Profiler
|
|||
import pytest
|
||||
import pybullet as p
|
||||
import numpy as np
|
||||
from gibson2.external.pybullet_tools.utils import set_base_values, joint_from_name, set_joint_position, \
|
||||
set_joint_positions, add_data_path, connect, plan_base_motion, plan_joint_motion, enable_gravity, \
|
||||
joint_controller, dump_body, load_model, joints_from_names, user_input, disconnect, get_joint_positions, \
|
||||
get_link_pose, link_from_name, HideOutput, get_pose, wait_for_user, dump_world, plan_nonholonomic_motion, set_point, create_box, stable_z, control_joints
|
||||
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
config = parse_config('../configs/jr_interactive_nav.yaml')
|
||||
s = Simulator(mode='headless', timestep=1 / 240.0)
|
||||
s = Simulator(mode='gui', 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])
|
||||
|
||||
obstacles = []
|
||||
obj = InteractiveObj(filename='/data4/mdv0/cabinet/0007/part_objs/cabinet_0007.urdf')
|
||||
s.import_interactive_object(obj)
|
||||
ids = s.import_interactive_object(obj)
|
||||
obstacles.append(ids)
|
||||
|
||||
obj.set_position([-2,0,0.5])
|
||||
obj = InteractiveObj(filename='/data4/mdv0/cabinet/0007/part_objs/cabinet_0007.urdf')
|
||||
s.import_interactive_object(obj)
|
||||
ids = s.import_interactive_object(obj)
|
||||
obstacles.append(ids)
|
||||
|
||||
obj.set_position([-2,2,0.5])
|
||||
obj = InteractiveObj(filename='/data4/mdv0/cabinet/0004/part_objs/cabinet_0004.urdf')
|
||||
s.import_interactive_object(obj)
|
||||
ids = s.import_interactive_object(obj)
|
||||
obstacles.append(ids)
|
||||
|
||||
obj.set_position([-2.1, 1.6, 2])
|
||||
obj = InteractiveObj(filename='/data4/mdv0/cabinet/0004/part_objs/cabinet_0004.urdf')
|
||||
s.import_interactive_object(obj)
|
||||
ids = s.import_interactive_object(obj)
|
||||
obstacles.append(ids)
|
||||
obj.set_position([-2.1, 0.4, 2])
|
||||
|
||||
obj = BoxShape([-2.05,1,0.5], [0.35,0.6,0.5])
|
||||
s.import_interactive_object(obj)
|
||||
ids = s.import_interactive_object(obj)
|
||||
obstacles.append(ids)
|
||||
|
||||
obj = BoxShape([-2.45,1,1.5], [0.01,2,1.5])
|
||||
s.import_interactive_object(obj)
|
||||
ids = s.import_interactive_object(obj)
|
||||
obstacles.append(ids)
|
||||
|
||||
p.createConstraint(0,-1,obj.body_id, -1, p.JOINT_FIXED, [0,0,1], [-2.55,1,1.5], [0,0,0])
|
||||
obj = YCBObject('003_cracker_box')
|
||||
s.import_object(obj)
|
||||
|
|
@ -59,11 +76,13 @@ while path is None:
|
|||
set_point(robot_id, [-3, -1, 0.0]) # Sets the z position of the robot
|
||||
set_joint_positions(robot_id, arm_joints, rest_position)
|
||||
|
||||
|
||||
path = plan_base_motion(robot_id, [-1,1.5,np.pi], ((-6, -6), (6, 6)), obstacles=obstacles, restarts=10, iterations=50, smooth=30)
|
||||
|
||||
for bq in path:
|
||||
set_base_values(robot_id, [bq[0], bq[1], bq[2]])
|
||||
set_joint_positions(robot_id, arm_joints, rest_position)
|
||||
|
||||
time.sleep(0.05)
|
||||
s.step()
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,291 @@
|
|||
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
|
||||
from gibson2.external.pybullet_tools.utils import set_base_values, joint_from_name, set_joint_position, \
|
||||
set_joint_positions, add_data_path, connect, plan_base_motion, plan_joint_motion, enable_gravity, \
|
||||
joint_controller, dump_body, load_model, joints_from_names, user_input, disconnect, get_joint_positions, \
|
||||
get_link_pose, link_from_name, HideOutput, get_pose, wait_for_user, dump_world, plan_nonholonomic_motion,\
|
||||
set_point, create_box, stable_z, control_joints, get_max_limits, get_min_limits
|
||||
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
config = parse_config('../configs/jr_interactive_nav.yaml')
|
||||
s = Simulator(mode='gui', timestep=1 / 240.0)
|
||||
scene = EmptyScene()
|
||||
s.import_scene(scene)
|
||||
fetch = Fetch(config)
|
||||
s.import_robot(fetch)
|
||||
fetch.robot_body.reset_position([0,0,0])
|
||||
fetch.robot_body.reset_orientation([0,0,1,0])
|
||||
|
||||
obstacles = []
|
||||
obj = InteractiveObj(filename='/data4/mdv0/cabinet/0007/part_objs/cabinet_0007.urdf')
|
||||
ids = s.import_interactive_object(obj)
|
||||
# for i in range(5):
|
||||
# p.changeDynamics(ids,i,lateralFriction=50,mass=0.1, linearDamping=0, angularDamping=0)
|
||||
obstacles.append(ids)
|
||||
|
||||
obj.set_position([-2,0,0.5])
|
||||
obj = InteractiveObj(filename='/data4/mdv0/cabinet/0007/part_objs/cabinet_0007.urdf')
|
||||
ids = s.import_interactive_object(obj)
|
||||
# for i in range(5):
|
||||
# p.changeDynamics(ids,i,lateralFriction=50,mass=0.1, linearDamping=0, angularDamping=0)
|
||||
obstacles.append(ids)
|
||||
|
||||
obj.set_position([-2,2,0.5])
|
||||
obj = InteractiveObj(filename='/data4/mdv0/cabinet/0004/part_objs/cabinet_0004.urdf')
|
||||
ids = s.import_interactive_object(obj)
|
||||
obstacles.append(ids)
|
||||
|
||||
obj.set_position([-2.1, 1.6, 2])
|
||||
obj = InteractiveObj(filename='/data4/mdv0/cabinet/0004/part_objs/cabinet_0004.urdf')
|
||||
ids = s.import_interactive_object(obj)
|
||||
obstacles.append(ids)
|
||||
obj.set_position([-2.1, 0.4, 2])
|
||||
|
||||
for ob in obstacles:
|
||||
#jointPositions = [0.000000, 0.000000, 0.000000, 0.000000, 0.000000]
|
||||
for jointIndex in range(p.getNumJoints(ob)):
|
||||
#p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
|
||||
friction = 1
|
||||
p.setJointMotorControl2(ob, jointIndex, p.VELOCITY_CONTROL, force=friction)
|
||||
|
||||
|
||||
obj = BoxShape([-2.05,1,0.5], [0.35,0.6,0.5])
|
||||
ids = s.import_interactive_object(obj)
|
||||
obstacles.append(ids)
|
||||
|
||||
obj = BoxShape([-2.45,1,1.5], [0.01,2,1.5])
|
||||
ids = s.import_interactive_object(obj)
|
||||
obstacles.append(ids)
|
||||
|
||||
p.createConstraint(0,-1,obj.body_id, -1, p.JOINT_FIXED, [0,0,1], [-2.55,1,1.5], [0,0,0])
|
||||
obj = YCBObject('003_cracker_box')
|
||||
s.import_object(obj)
|
||||
p.resetBasePositionAndOrientation(obj.body_id, [-2,1,1.2], [0,0,0,1])
|
||||
obj = YCBObject('003_cracker_box')
|
||||
s.import_object(obj)
|
||||
p.resetBasePositionAndOrientation(obj.body_id, [-2,2,1.2], [0,0,0,1])
|
||||
|
||||
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'])
|
||||
rest_position = (0.38548146667743244, 1.1522793897208579, 1.2576467971105596, -0.312703569911879,
|
||||
1.7404867100093226, -0.0962895617312548, -1.4418232619629425, -1.6780152866247762)
|
||||
finger_joints = joints_from_names(robot_id, ['l_gripper_finger_joint', 'r_gripper_finger_joint'])
|
||||
|
||||
path = None
|
||||
set_joint_positions(robot_id, arm_joints, rest_position)
|
||||
if 0:
|
||||
while path is None:
|
||||
set_point(robot_id, [-3, -1, 0.0]) # Sets the z position of the robot
|
||||
set_joint_positions(robot_id, arm_joints, rest_position)
|
||||
control_joints(robot_id, finger_joints, [0.04, 0.04])
|
||||
path = plan_base_motion(robot_id, [-1,1.5,np.pi], ((-6, -6), (6, 6)), obstacles=obstacles, restarts=10, iterations=50, smooth=30)
|
||||
|
||||
for bq in path:
|
||||
set_base_values(robot_id, [bq[0], bq[1], bq[2]])
|
||||
set_joint_positions(robot_id, arm_joints, rest_position)
|
||||
control_joints(robot_id, finger_joints, [0.04, 0.04])
|
||||
time.sleep(0.05)
|
||||
s.step()
|
||||
|
||||
|
||||
set_base_values(robot_id, [-1,1.5,np.pi])
|
||||
|
||||
x,y,z = (-1.5234082112189532, 1.8056819568596753, 0.9170480480678451) #fetch.get_end_effector_position()
|
||||
print(x,y,z)
|
||||
visual_marker = p.createVisualShape(p.GEOM_SPHERE, radius = 0.02, rgbaColor=(1,1,1,0.5))
|
||||
marker = p.createMultiBody(baseVisualShapeIndex = visual_marker)
|
||||
f = 1
|
||||
#control_joints(robot_id, finger_joints, [f,f])
|
||||
|
||||
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_ik = [0,0] + list(get_joint_positions(robot_id, arm_joints)) + [0.04,0.04]
|
||||
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)
|
||||
|
||||
for i in range(100):
|
||||
set_joint_positions(robot_id, arm_joints, rest_position)
|
||||
control_joints(robot_id, finger_joints, [0.04, 0.04])
|
||||
s.step()
|
||||
|
||||
p.changeDynamics(robot_id, fetch.parts['r_gripper_finger_link'].body_part_index, lateralFriction=1)
|
||||
p.changeDynamics(robot_id, fetch.parts['l_gripper_finger_link'].body_part_index, lateralFriction=1)
|
||||
|
||||
if 1:
|
||||
sid = p.saveState()
|
||||
joint_pos = p.calculateInverseKinematics(robot_id, fetch.parts['gripper_link'].body_part_index, [x, y, z],
|
||||
p.getQuaternionFromEuler([np.pi/2,-np.pi,0]),
|
||||
lowerLimits=min_limits,
|
||||
upperLimits=max_limits,
|
||||
jointRanges=joint_range,
|
||||
restPoses=rest_position_ik,
|
||||
jointDamping=jd,
|
||||
solver=p.IK_DLS,
|
||||
maxNumIterations=100)[2:10]
|
||||
|
||||
arm_path = plan_joint_motion(robot_id, arm_joints, joint_pos, disabled_collisions=set(),
|
||||
self_collisions=True)
|
||||
p.restoreState(sid)
|
||||
|
||||
for jp in arm_path:
|
||||
set_base_values(robot_id, [-1, 1.5, np.pi])
|
||||
|
||||
for i in range(10):
|
||||
control_joints(robot_id, finger_joints, [0.04, 0.04])
|
||||
control_joints(robot_id, arm_joints, jp)
|
||||
s.step()
|
||||
|
||||
|
||||
sid = p.saveState()
|
||||
joint_pos = p.calculateInverseKinematics(robot_id, fetch.parts['gripper_link'].body_part_index,
|
||||
[x-0.10, y, z],
|
||||
p.getQuaternionFromEuler([np.pi / 2, -np.pi, 0]),
|
||||
lowerLimits=min_limits,
|
||||
upperLimits=max_limits,
|
||||
jointRanges=joint_range,
|
||||
restPoses=rest_position_ik,
|
||||
jointDamping=jd,
|
||||
solver=p.IK_DLS,
|
||||
maxNumIterations=100)[2:10]
|
||||
|
||||
arm_path = plan_joint_motion(robot_id, arm_joints, joint_pos, disabled_collisions=set(),
|
||||
self_collisions=True)
|
||||
p.restoreState(sid)
|
||||
|
||||
for jp in arm_path:
|
||||
set_base_values(robot_id, [-1, 1.5, np.pi])
|
||||
|
||||
for i in range(10):
|
||||
control_joints(robot_id, finger_joints, [0.04, 0.04])
|
||||
control_joints(robot_id, arm_joints, jp)
|
||||
s.step()
|
||||
|
||||
p.setJointMotorControl2(robot_id, joint_from_name(robot_id, 'l_gripper_finger_joint'), p.VELOCITY_CONTROL, targetVelocity=-0.2, force=500)
|
||||
p.setJointMotorControl2(robot_id, joint_from_name(robot_id, 'r_gripper_finger_joint'), p.VELOCITY_CONTROL, targetVelocity=-0.2, force=500)
|
||||
|
||||
for i in range(150):
|
||||
set_base_values(robot_id, [-1, 1.5, np.pi])
|
||||
s.step()
|
||||
|
||||
sid = p.saveState()
|
||||
x,y,z = fetch.parts['gripper_link'].get_pose()[:3]
|
||||
joint_pos = p.calculateInverseKinematics(robot_id, fetch.parts['gripper_link'].body_part_index,
|
||||
[x + 0.1, y, z],
|
||||
p.getQuaternionFromEuler([np.pi / 2, -np.pi, 0]),
|
||||
lowerLimits=min_limits,
|
||||
upperLimits=max_limits,
|
||||
jointRanges=joint_range,
|
||||
restPoses=rest_position_ik,
|
||||
jointDamping=jd,
|
||||
solver=p.IK_DLS,
|
||||
maxNumIterations=1000)[2:10]
|
||||
|
||||
arm_path = plan_joint_motion(robot_id, arm_joints, joint_pos, disabled_collisions=set(),
|
||||
self_collisions=True)
|
||||
|
||||
p.restoreState(sid)
|
||||
|
||||
for jp in arm_path:
|
||||
set_base_values(robot_id, [-1, 1.5, np.pi])
|
||||
|
||||
for i in range(10):
|
||||
control_joints(robot_id, arm_joints, jp)
|
||||
s.step()
|
||||
|
||||
sid = p.saveState()
|
||||
x, y, z = fetch.parts['gripper_link'].get_pose()[:3]
|
||||
joint_pos = p.calculateInverseKinematics(robot_id, fetch.parts['gripper_link'].body_part_index,
|
||||
[x + 0.1, y, z],
|
||||
p.getQuaternionFromEuler([np.pi / 2, -np.pi, 0]),
|
||||
lowerLimits=min_limits,
|
||||
upperLimits=max_limits,
|
||||
jointRanges=joint_range,
|
||||
restPoses=rest_position_ik,
|
||||
jointDamping=jd,
|
||||
solver=p.IK_DLS,
|
||||
maxNumIterations=100)[2:10]
|
||||
|
||||
arm_path = plan_joint_motion(robot_id, arm_joints, joint_pos, disabled_collisions=set(),
|
||||
self_collisions=True)
|
||||
|
||||
p.restoreState(sid)
|
||||
|
||||
for jp in arm_path:
|
||||
set_base_values(robot_id, [-1, 1.5, np.pi])
|
||||
|
||||
for i in range(10):
|
||||
control_joints(robot_id, arm_joints, jp)
|
||||
s.step()
|
||||
|
||||
sid = p.saveState()
|
||||
x, y, z = fetch.parts['gripper_link'].get_pose()[:3]
|
||||
joint_pos = p.calculateInverseKinematics(robot_id, fetch.parts['gripper_link'].body_part_index,
|
||||
[x + 0.1, y, z],
|
||||
p.getQuaternionFromEuler([np.pi / 2, -np.pi, 0]),
|
||||
lowerLimits=min_limits,
|
||||
upperLimits=max_limits,
|
||||
jointRanges=joint_range,
|
||||
restPoses=rest_position_ik,
|
||||
jointDamping=jd,
|
||||
solver=p.IK_DLS,
|
||||
maxNumIterations=100)[2:10]
|
||||
|
||||
arm_path = plan_joint_motion(robot_id, arm_joints, joint_pos, disabled_collisions=set(),
|
||||
self_collisions=True)
|
||||
|
||||
p.restoreState(sid)
|
||||
|
||||
for jp in arm_path:
|
||||
set_base_values(robot_id, [-1, 1.5, np.pi])
|
||||
|
||||
for i in range(10):
|
||||
control_joints(robot_id, arm_joints, jp)
|
||||
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
|
||||
if (k == ord('a') and (v & p.KEY_IS_DOWN)):
|
||||
f += 0.01
|
||||
if (k == ord('d') and (v & p.KEY_IS_DOWN)):
|
||||
f -= 0.01
|
||||
if (k == ord('p') and (v & p.KEY_IS_DOWN)):
|
||||
print(x,y,z)
|
||||
|
||||
p.resetBasePositionAndOrientation(marker, [x,y,z], [0,0,0,1])
|
||||
p.setJointMotorControl2(robot_id, joint_from_name(robot_id, 'l_wheel_joint'), p.VELOCITY_CONTROL, targetVelocity=0, force=1000)
|
||||
p.setJointMotorControl2(robot_id, joint_from_name(robot_id, 'r_wheel_joint'), p.VELOCITY_CONTROL, targetVelocity=0, force=1000)
|
||||
|
||||
while True:
|
||||
set_base_values(robot_id, [-1, 1.5, np.pi])
|
||||
s.step()
|
||||
|
||||
|
||||
s.disconnect()
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
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
|
||||
from gibson2.external.pybullet_tools.utils import set_base_values, joint_from_name, set_joint_position, \
|
||||
set_joint_positions, add_data_path, connect, plan_base_motion, plan_joint_motion, enable_gravity, \
|
||||
joint_controller, dump_body, load_model, joints_from_names, user_input, disconnect, get_joint_positions, \
|
||||
get_link_pose, link_from_name, HideOutput, get_pose, wait_for_user, dump_world, plan_nonholonomic_motion, set_point, create_box, stable_z
|
||||
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
config = parse_config('../configs/jr_interactive_nav.yaml')
|
||||
s = Simulator(mode='gui', timestep=1 / 240.0)
|
||||
scene = EmptyScene()
|
||||
s.import_scene(scene)
|
||||
jr = JR2_Kinova(config)
|
||||
s.import_robot(jr)
|
||||
|
||||
robot_id = jr.robot_ids[0]
|
||||
jr_end_effector_link_id = 33
|
||||
|
||||
#arm_joints = joints_from_names(robot_id, ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_flex_joint', 'wrist_flex_joint'])
|
||||
arm_joints = joints_from_names(robot_id, ['m1n6s200_joint_1', 'm1n6s200_joint_2', 'm1n6s200_joint_3', 'm1n6s200_joint_4', 'm1n6s200_joint_5'])
|
||||
finger_joints = joints_from_names(robot_id, ['m1n6s200_joint_finger_1', 'm1n6s200_joint_finger_2'])
|
||||
jr.robot_body.reset_position([0, 0, 0])
|
||||
jr.robot_body.reset_orientation([0, 0, 1, 0])
|
||||
rest_position = [-1.6214899194372223, 1.4082722179709484, -2.9650918084213216, -1.7071872988002772, 3.0503822148927712e-05]
|
||||
set_joint_positions(robot_id, arm_joints, rest_position)
|
||||
x,y,z = jr.get_end_effector_position()
|
||||
f = 1
|
||||
set_joint_positions(robot_id, finger_joints, [f,f])
|
||||
|
||||
print(x,y,z)
|
||||
visual_marker = p.createVisualShape(p.GEOM_SPHERE, radius = 0.02)
|
||||
marker = p.createMultiBody(baseVisualShapeIndex = visual_marker)
|
||||
|
||||
while True:
|
||||
with Profiler("Simulation: step"):
|
||||
jr.robot_body.reset_position([0, 0, 0])
|
||||
jr.robot_body.reset_orientation([0, 0, 1, 0])
|
||||
joint_pos = p.calculateInverseKinematics(robot_id, 33, [x, y, z])[2:7]
|
||||
print(x,y,z)
|
||||
##z += 0.002
|
||||
set_joint_positions(robot_id, arm_joints, joint_pos)
|
||||
set_joint_positions(robot_id, finger_joints, [f, f])
|
||||
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
|
||||
if (k == ord('a') and (v & p.KEY_IS_DOWN)):
|
||||
f += 0.01
|
||||
if (k == ord('s') and (v & p.KEY_IS_DOWN)):
|
||||
f -= 0.01
|
||||
|
||||
p.resetBasePositionAndOrientation(marker, [x,y,z], [0,0,0,1])
|
||||
|
||||
#print(joint_pos)
|
||||
s.disconnect()
|
||||
|
|
@ -0,0 +1,105 @@
|
|||
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
|
||||
from gibson2.external.pybullet_tools.utils import set_base_values, joint_from_name, set_joint_position, \
|
||||
set_joint_positions, add_data_path, connect, plan_base_motion, plan_joint_motion, enable_gravity, \
|
||||
joint_controller, dump_body, load_model, joints_from_names, user_input, disconnect, get_joint_positions, \
|
||||
get_link_pose, link_from_name, HideOutput, get_pose, wait_for_user, dump_world, plan_nonholonomic_motion, \
|
||||
set_point, create_box, stable_z, control_joints, get_max_limits, get_min_limits, get_sample_fn
|
||||
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
config = parse_config('../configs/jr_interactive_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, ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_flex_joint', 'wrist_flex_joint'])
|
||||
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]
|
||||
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())
|
||||
closeEnough = False
|
||||
iter = 0
|
||||
dist2 = 1e30
|
||||
while (not closeEnough and iter < 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]
|
||||
diff = [targetPos[0] - newPos[0], targetPos[1] - newPos[1], targetPos[2] - newPos[2]]
|
||||
dist2 = (diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2])
|
||||
closeEnough = (dist2 < threshold)
|
||||
iter = iter + 1
|
||||
print ("Num iter: "+str(iter) + " threshold: "+str(dist2))
|
||||
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.001
|
||||
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()
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
from gibson2.envs.motion_planner_env import MotionPlanningBaseArmEnv
|
||||
from gibson2.core.physics.interactive_objects import VisualMarker, InteractiveObj, BoxShape
|
||||
import gibson2
|
||||
import argparse, time
|
||||
from transforms3d.quaternions import quat2mat, qmult
|
||||
from transforms3d.euler import euler2quat
|
||||
from gibson2.utils.utils import parse_config, rotate_vector_3d, rotate_vector_2d, l2_distance, quatToXYZW
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import cv2
|
||||
from gibson2.external.pybullet_tools.utils import *
|
||||
import sys
|
||||
|
||||
config_file = '../../examples/configs/fetch_interactive_nav_s2r_mp.yaml'
|
||||
mode = 'gui'
|
||||
|
||||
nav_env = MotionPlanningBaseArmEnv(config_file=config_file,
|
||||
model_id=sys.argv[1],
|
||||
mode=mode,
|
||||
action_timestep=1.0 / 500.0,
|
||||
physics_timestep=1.0 / 500.0,
|
||||
eval=True, arena='button_door')
|
||||
|
||||
nav_env.reset()
|
||||
#for i in range(10):
|
||||
# nav_env.simulator_step()
|
||||
|
||||
for pose, mass, color in \
|
||||
zip(nav_env.semantic_obstacle_poses, nav_env.semantic_obstacle_masses, nav_env.semantic_obstacle_colors):
|
||||
obstacle = BoxShape(pos=pose, dim=[nav_env.obstacle_dim, nav_env.obstacle_dim, 0.6], mass=mass, color=color)
|
||||
nav_env.simulator.import_interactive_object(obstacle, class_id=4)
|
||||
p.changeDynamics(obstacle.body_id, -1, lateralFriction=0.5)
|
||||
|
||||
|
||||
|
||||
def draw_box(x1,x2,y1,y2, perm=False):
|
||||
if perm:
|
||||
t = 0
|
||||
else:
|
||||
t = 10
|
||||
|
||||
p.addUserDebugLine([x1,y1,0.5], [x2,y2,0.5], lineWidth=3, lifeTime=t)
|
||||
p.addUserDebugLine([x1,y2,0.5], [x2,y1,0.5], lineWidth=3, lifeTime=t)\
|
||||
|
||||
p.addUserDebugLine([x1,y1,0.5], [x1,y2,0.5], lineWidth=3, lifeTime=t)
|
||||
p.addUserDebugLine([x2,y1,0.5], [x2,y2,0.5], lineWidth=3, lifeTime=t)
|
||||
|
||||
p.addUserDebugLine([x1,y1,0.5], [x2,y1,0.5], lineWidth=3, lifeTime=t)
|
||||
p.addUserDebugLine([x1,y2,0.5], [x2,y2,0.5], lineWidth=3, lifeTime=t)
|
||||
|
||||
def draw_text(x, y, text):
|
||||
p.addUserDebugText(text, [x,y,0.5])
|
||||
|
||||
|
||||
|
||||
draw_box(nav_env.initial_pos_range[0][0], nav_env.initial_pos_range[0][1],
|
||||
nav_env.initial_pos_range[1][0], nav_env.initial_pos_range[1][1], perm=True)
|
||||
draw_text((nav_env.initial_pos_range[0][0] + nav_env.initial_pos_range[0][1])/2,
|
||||
(nav_env.initial_pos_range[1][0] + nav_env.initial_pos_range[1][1])/2, 'start range')
|
||||
|
||||
|
||||
draw_box(nav_env.target_pos_range[0][0], nav_env.target_pos_range[0][1],
|
||||
nav_env.target_pos_range[1][0], nav_env.target_pos_range[1][1], perm=True)
|
||||
draw_text((nav_env.target_pos_range[0][0] + nav_env.target_pos_range[0][1])/2,
|
||||
(nav_env.target_pos_range[1][0] + nav_env.target_pos_range[1][1])/2, 'target range')
|
||||
|
||||
|
||||
for door_pos_range in nav_env.door_target_pos:
|
||||
|
||||
draw_box(door_pos_range[0][0], door_pos_range[0][1],
|
||||
door_pos_range[1][0], door_pos_range[1][1], perm=True)
|
||||
|
||||
draw_text((door_pos_range[0][0] + door_pos_range[0][1])/2,
|
||||
(door_pos_range[1][0] + door_pos_range[1][1])/2, 'door target range')
|
||||
|
||||
|
||||
while True:
|
||||
nav_env.simulator_step()
|
||||
|
|
@ -158,21 +158,25 @@ class VisualMarker(object):
|
|||
|
||||
|
||||
class BoxShape(object):
|
||||
def __init__(self, pos=[1, 2, 3], dim=[1, 2, 3]):
|
||||
def __init__(self, pos=[1, 2, 3], dim=[1, 2, 3], visual_only=False, mass=1000, color=[1, 1, 1, 1]):
|
||||
self.basePos = pos
|
||||
self.dimension = dim
|
||||
self.body_id = None
|
||||
self.visual_only = visual_only
|
||||
self.mass = mass
|
||||
self.color = color
|
||||
|
||||
def load(self):
|
||||
mass = 1000
|
||||
baseOrientation = [0, 0, 0, 1]
|
||||
|
||||
colBoxId = p.createCollisionShape(p.GEOM_BOX, halfExtents=self.dimension)
|
||||
visualShapeId = p.createVisualShape(p.GEOM_BOX, halfExtents=self.dimension)
|
||||
|
||||
self.body_id = p.createMultiBody(baseMass=mass,
|
||||
baseCollisionShapeIndex=colBoxId,
|
||||
baseVisualShapeIndex=visualShapeId)
|
||||
visualShapeId = p.createVisualShape(p.GEOM_BOX, halfExtents=self.dimension, rgbaColor=self.color)
|
||||
if self.visual_only:
|
||||
self.body_id = p.createMultiBody(baseCollisionShapeIndex=-1,
|
||||
baseVisualShapeIndex=visualShapeId)
|
||||
else:
|
||||
self.body_id = p.createMultiBody(baseMass=self.mass,
|
||||
baseCollisionShapeIndex=colBoxId,
|
||||
baseVisualShapeIndex=visualShapeId)
|
||||
|
||||
p.resetBasePositionAndOrientation(self.body_id, self.basePos, baseOrientation)
|
||||
return self.body_id
|
||||
|
|
|
|||
|
|
@ -106,6 +106,7 @@ class BaseRobot:
|
|||
force=0)
|
||||
_, joint_name, joint_type, _, _, _, _, _, _, _, _, _, part_name, _, _, _, _ = p.getJointInfo(
|
||||
bodies[0], j)
|
||||
print(p.getJointInfo(bodies[0], j))
|
||||
joint_name = joint_name.decode("utf8")
|
||||
part_name = part_name.decode("utf8")
|
||||
|
||||
|
|
@ -252,7 +253,7 @@ class BodyPart:
|
|||
if link_id == -1:
|
||||
(x, y, z), (a, b, c, d) = p.getBasePositionAndOrientation(body_id)
|
||||
else:
|
||||
(x, y, z), (a, b, c, d), _, _, _, _ = p.getLinkState(body_id, link_id)
|
||||
_, _, _, _, (x, y, z), (a, b, c, d) = p.getLinkState(body_id, link_id)
|
||||
x, y, z = x * self.scale, y * self.scale, z * self.scale
|
||||
return np.array([x, y, z, a, b, c, d])
|
||||
|
||||
|
|
@ -356,14 +357,6 @@ class Joint:
|
|||
vx *= self.scale
|
||||
return x, vx, trq
|
||||
|
||||
def set_state(self, x, vx):
|
||||
"""Set state of joint
|
||||
x is defined in real world scale """
|
||||
if self.joint_type == p.JOINT_PRISMATIC:
|
||||
x /= self.scale
|
||||
vx /= self.scale
|
||||
p.resetJointState(self.bodies[self.body_index], self.joint_index, x, vx)
|
||||
|
||||
def get_relative_state(self):
|
||||
pos, vel, trq = self.get_state()
|
||||
|
||||
|
|
@ -405,7 +398,21 @@ class Joint:
|
|||
force=torque) # , positionGain=0.1, velocityGain=0.1)
|
||||
|
||||
def reset_state(self, pos, vel):
|
||||
self.set_state(pos, vel)
|
||||
"""
|
||||
Reset pos and vel of joint
|
||||
"""
|
||||
if self.joint_type == p.JOINT_PRISMATIC:
|
||||
pos /= self.scale
|
||||
vel /= self.scale
|
||||
|
||||
# resetJointState does not reset targetVelocity correctly, so we need to also call setJointMotorControl2.
|
||||
p.resetJointState(self.bodies[self.body_index], self.joint_index, targetValue=pos, targetVelocity=vel)
|
||||
p.setJointMotorControl2(bodyIndex=self.bodies[self.body_index],
|
||||
jointIndex=self.joint_index,
|
||||
controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=pos,
|
||||
targetVelocity=vel,
|
||||
force=0)
|
||||
|
||||
def disable_motor(self):
|
||||
p.setJointMotorControl2(self.bodies[self.body_index],
|
||||
|
|
|
|||
|
|
@ -8,7 +8,11 @@ from transforms3d.euler import euler2quat, euler2mat
|
|||
from transforms3d.quaternions import quat2mat, qmult
|
||||
import transforms3d.quaternions as quat
|
||||
import sys
|
||||
|
||||
from gibson2.external.pybullet_tools.utils import set_base_values, joint_from_name, set_joint_position, \
|
||||
set_joint_positions, add_data_path, connect, plan_base_motion, plan_joint_motion, enable_gravity, \
|
||||
joint_controller, dump_body, load_model, joints_from_names, user_input, disconnect, get_joint_positions, \
|
||||
get_link_pose, link_from_name, HideOutput, get_pose, wait_for_user, dump_world, plan_nonholonomic_motion, \
|
||||
set_point, create_box, stable_z, control_joints, get_max_limits, get_min_limits, get_base_values
|
||||
|
||||
class LocomotorRobot(BaseRobot):
|
||||
""" Built on top of BaseRobot
|
||||
|
|
@ -490,8 +494,8 @@ class Freight(LocomotorRobot):
|
|||
self.config = config
|
||||
self.velocity = config.get("velocity", 1.0)
|
||||
LocomotorRobot.__init__(self,
|
||||
"fetch/freight.urdf",
|
||||
"base_link",
|
||||
"fetch/freight.urdf",
|
||||
"base_link",
|
||||
action_dim=2,
|
||||
sensor_dim=16,
|
||||
power=2.5,
|
||||
|
|
@ -537,29 +541,37 @@ class Fetch(LocomotorRobot):
|
|||
|
||||
def __init__(self, config):
|
||||
self.config = config
|
||||
self.velocity = config.get("velocity", 1.0)
|
||||
self.wheel_velocity = config.get('wheel_velocity', 0.1)
|
||||
self.torso_lift_velocity = config.get('torso_lift_velocity', 0.01)
|
||||
self.arm_velocity = config.get('arm_velocity', 0.01)
|
||||
self.wheel_dim = 2
|
||||
self.torso_lift_dim = 1
|
||||
self.arm_dim = 7
|
||||
self.action_high = config.get("action_high", None)
|
||||
self.action_low = config.get("action_low", None)
|
||||
LocomotorRobot.__init__(self,
|
||||
"fetch/fetch.urdf",
|
||||
"base_link",
|
||||
action_dim=6,
|
||||
"fetch/fetch.urdf",
|
||||
"base_link",
|
||||
action_dim=self.wheel_dim + self.torso_lift_dim + self.arm_dim,
|
||||
sensor_dim=55,
|
||||
power=2.5,
|
||||
scale=config.get("robot_scale", self.default_scale),
|
||||
resolution=config.get("resolution", 64),
|
||||
is_discrete=config.get("is_discrete", True),
|
||||
control="velocity")
|
||||
control="velocity",
|
||||
self_collision=True)
|
||||
|
||||
def set_up_continuous_action_space(self):
|
||||
if self.action_high is not None and self.action_low is not None:
|
||||
self.action_high = np.full(shape=self.wheel_dim, fill_value=self.action_high)
|
||||
self.action_low = np.full(shape=self.wheel_dim, fill_value=self.action_low)
|
||||
else:
|
||||
self.action_high = np.full(shape=self.wheel_dim, fill_value=self.velocity)
|
||||
self.action_high = np.array([self.wheel_velocity] * self.wheel_dim +
|
||||
[self.torso_lift_velocity] * self.torso_lift_dim +
|
||||
[self.arm_velocity] * self.arm_dim)
|
||||
self.action_low = -self.action_high
|
||||
self.action_space = gym.spaces.Box(shape=(self.wheel_dim,),
|
||||
|
||||
self.action_space = gym.spaces.Box(shape=(self.action_dim,),
|
||||
low=-1.0,
|
||||
high=1.0,
|
||||
dtype=np.float32)
|
||||
|
|
@ -569,14 +581,39 @@ class Fetch(LocomotorRobot):
|
|||
|
||||
def robot_specific_reset(self):
|
||||
super(Fetch, self).robot_specific_reset()
|
||||
for j in self.ordered_joints:
|
||||
j.reset_joint_state(0.0, 0.0)
|
||||
# roll the arm to its body
|
||||
for i in range(2, 6):
|
||||
self.ordered_joints[i].reset_joint_state(np.pi / 2.0, 0.0)
|
||||
robot_id = self.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'
|
||||
])
|
||||
# rest_position = (
|
||||
# 0.02, np.pi / 2.0,
|
||||
# np.pi / 2.0, 0.0,
|
||||
# np.pi / 2.0 + 0.1, 0.0,
|
||||
# np.pi / 2.0, 0.0
|
||||
# )
|
||||
# rest_position = (0.38548146667743244, 1.1522793897208579, 1.2576467971105596, -0.312703569911879,
|
||||
# 1.7404867100093226, -0.0962895617312548, -1.4418232619629425, -1.6780152866247762)
|
||||
rest_position = (0.30322468280792236, -1.414019864768982,
|
||||
1.5178184935241699, 0.8189625336474915,
|
||||
2.200358942909668, 2.9631312579803466,
|
||||
-1.2862852996643066, 0.0008453550418615341)
|
||||
|
||||
set_joint_positions(robot_id, arm_joints, rest_position)
|
||||
|
||||
|
||||
def apply_action(self, action):
|
||||
denormalized_action = self.action_to_real_action(action)
|
||||
real_action = np.zeros(self.action_dim)
|
||||
real_action[:self.wheel_dim] = denormalized_action
|
||||
real_action = self.action_to_real_action(action)
|
||||
self.apply_real_action(real_action)
|
||||
|
||||
def calc_state(self):
|
||||
|
|
@ -585,6 +622,26 @@ class Fetch(LocomotorRobot):
|
|||
print(len(base_state), len(angular_velocity))
|
||||
return np.concatenate((base_state, np.array(angular_velocity)))
|
||||
|
||||
def get_end_effector_position(self):
|
||||
return self.parts['gripper_link'].get_position()
|
||||
|
||||
def load(self):
|
||||
ids = self._load_model()
|
||||
self.eyes = self.parts["eyes"]
|
||||
|
||||
robot_id = ids[0]
|
||||
|
||||
# disable collision for immediate parent
|
||||
for joint in range(p.getNumJoints(robot_id)):
|
||||
info = p.getJointInfo(robot_id, joint)
|
||||
parent_id = info[-1]
|
||||
p.setCollisionFilterPair(robot_id, robot_id, joint, parent_id, 0)
|
||||
|
||||
# disable collision for torso_lift_joint and shoulder_lift_joint
|
||||
p.setCollisionFilterPair(robot_id, robot_id, 3, 13, 0)
|
||||
|
||||
return ids
|
||||
|
||||
|
||||
class JR2(LocomotorRobot):
|
||||
mjcf_scaling = 1
|
||||
|
|
@ -631,7 +688,6 @@ class JR2(LocomotorRobot):
|
|||
}
|
||||
|
||||
|
||||
# TODO: set up joint id and name mapping
|
||||
class JR2_Kinova(LocomotorRobot):
|
||||
mjcf_scaling = 1
|
||||
model_type = "URDF"
|
||||
|
|
@ -709,3 +765,70 @@ class JR2_Kinova(LocomotorRobot):
|
|||
for j in range(16, 28):
|
||||
p.setCollisionFilterPair(robot_id, robot_id, joint, j, 0)
|
||||
return ids
|
||||
|
||||
|
||||
class Locobot(LocomotorRobot):
|
||||
mjcf_scaling = 1
|
||||
model_type = "URDF"
|
||||
default_scale = 1
|
||||
|
||||
def __init__(self, config):
|
||||
self.config = config
|
||||
self.wheel_velocity = config.get('wheel_velocity', 0.1)
|
||||
self.wheel_dim = 2
|
||||
self.action_high = config.get("action_high", None)
|
||||
self.action_low = config.get("action_low", None)
|
||||
LocomotorRobot.__init__(self,
|
||||
"locobot/locobot.urdf",
|
||||
"base_link",
|
||||
action_dim=self.wheel_dim,
|
||||
sensor_dim=55, # TODO: what is sensor_dim?
|
||||
power=2.5,
|
||||
scale=config.get("robot_scale", self.default_scale),
|
||||
resolution=config.get("resolution", 64),
|
||||
is_discrete=config.get("is_discrete", True),
|
||||
control="velocity",
|
||||
self_collision=False)
|
||||
|
||||
def set_up_continuous_action_space(self):
|
||||
if self.action_high is not None and self.action_low is not None:
|
||||
self.action_high = np.full(shape=self.wheel_dim, fill_value=self.action_high)
|
||||
self.action_low = np.full(shape=self.wheel_dim, fill_value=self.action_low)
|
||||
else:
|
||||
self.action_high = np.array([self.wheel_velocity] * self.wheel_dim)
|
||||
self.action_low = -self.action_high
|
||||
|
||||
self.action_space = gym.spaces.Box(shape=(self.action_dim,),
|
||||
low=-1.0,
|
||||
high=1.0,
|
||||
dtype=np.float32)
|
||||
|
||||
def set_up_discrete_action_space(self):
|
||||
assert False, "Locobot does not support discrete actions"
|
||||
|
||||
def apply_action(self, action):
|
||||
real_action = self.action_to_real_action(action)
|
||||
self.apply_real_action(real_action)
|
||||
|
||||
def calc_state(self):
|
||||
base_state = LocomotorRobot.calc_state(self)
|
||||
angular_velocity = self.robot_body.angular_velocity()
|
||||
print(len(base_state), len(angular_velocity))
|
||||
return np.concatenate((base_state, np.array(angular_velocity)))
|
||||
|
||||
def get_end_effector_position(self):
|
||||
return self.parts['gripper_link'].get_position()
|
||||
|
||||
def load(self):
|
||||
ids = self._load_model()
|
||||
self.eyes = self.parts["eyes"]
|
||||
|
||||
robot_id = ids[0]
|
||||
|
||||
# disable collision for immediate parent
|
||||
for joint in range(p.getNumJoints(robot_id)):
|
||||
info = p.getJointInfo(robot_id, joint)
|
||||
parent_id = info[-1]
|
||||
p.setCollisionFilterPair(robot_id, robot_id, joint, parent_id, 0)
|
||||
|
||||
return ids
|
||||
|
|
|
|||
|
|
@ -11,6 +11,7 @@ from PIL import Image
|
|||
import cv2
|
||||
import networkx as nx
|
||||
from IPython import embed
|
||||
import pickle
|
||||
|
||||
class Scene:
|
||||
def load(self):
|
||||
|
|
@ -23,6 +24,7 @@ class EmptyScene(Scene):
|
|||
def load(self):
|
||||
planeName = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
|
||||
self.ground_plane_mjcf = p.loadMJCF(planeName)
|
||||
p.changeDynamics(self.ground_plane_mjcf[0], -1, lateralFriction=1)
|
||||
return [item for item in self.ground_plane_mjcf]
|
||||
|
||||
class StadiumScene(Scene):
|
||||
|
|
@ -46,6 +48,9 @@ class StadiumScene(Scene):
|
|||
def get_random_point(self, random_height=False):
|
||||
return self.get_random_point_floor(0, random_height)
|
||||
|
||||
def get_random_floor(self):
|
||||
return 0
|
||||
|
||||
def get_random_point_floor(self, floor, random_height=False):
|
||||
del floor
|
||||
return 0, np.array([
|
||||
|
|
@ -54,6 +59,13 @@ class StadiumScene(Scene):
|
|||
np.random.uniform(0.4, 0.8) if random_height else 0.0
|
||||
])
|
||||
|
||||
def reset_floor(self, floor=0, additional_elevation=0.05, height=None):
|
||||
return
|
||||
|
||||
def get_floor_height(self, floor):
|
||||
return 0.0
|
||||
|
||||
|
||||
|
||||
class InteractiveBuildingScene(Scene):
|
||||
"""
|
||||
|
|
@ -69,6 +81,27 @@ class InteractiveBuildingScene(Scene):
|
|||
return [body_id]
|
||||
|
||||
|
||||
|
||||
class StadiumSceneInteractive(Scene):
|
||||
zero_at_running_strip_start_line = True # if False, center of coordinates (0,0,0) will be at the middle of the stadium
|
||||
stadium_halflen = 105 * 0.25 # FOOBALL_FIELD_HALFLEN
|
||||
stadium_halfwidth = 50 * 0.25 # FOOBALL_FIELD_HALFWID
|
||||
|
||||
def load(self):
|
||||
filename = os.path.join(pybullet_data.getDataPath(), "stadium_no_collision.sdf")
|
||||
self.stadium = p.loadSDF(filename)
|
||||
planeName = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
|
||||
self.ground_plane_mjcf = p.loadMJCF(planeName)
|
||||
for i in self.ground_plane_mjcf:
|
||||
pos, orn = p.getBasePositionAndOrientation(i)
|
||||
p.resetBasePositionAndOrientation(i, [pos[0], pos[1], pos[2] - 0.005], orn)
|
||||
|
||||
for i in self.ground_plane_mjcf:
|
||||
p.changeVisualShape(i, -1, rgbaColor=[1, 1, 1, 0.5])
|
||||
|
||||
return [item for item in self.stadium] + [item for item in self.ground_plane_mjcf]
|
||||
|
||||
|
||||
class BuildingScene(Scene):
|
||||
"""
|
||||
Gibson Environment building scenes
|
||||
|
|
@ -104,6 +137,7 @@ class BuildingScene(Scene):
|
|||
self.should_load_replaced_objects = should_load_replaced_objects
|
||||
self.num_waypoints = num_waypoints
|
||||
self.waypoint_interval = int(waypoint_resolution / trav_map_resolution)
|
||||
self.mesh_body_id = None
|
||||
|
||||
def l2_distance(self, a, b):
|
||||
return np.linalg.norm(np.array(a) - np.array(b))
|
||||
|
|
@ -121,13 +155,24 @@ class BuildingScene(Scene):
|
|||
else:
|
||||
filename = os.path.join(get_model_path(self.model_id), "mesh_z_up.obj")
|
||||
scaling = [1, 1, 1]
|
||||
|
||||
collisionId = p.createCollisionShape(p.GEOM_MESH,
|
||||
fileName=filename,
|
||||
meshScale=scaling,
|
||||
flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
|
||||
visualId = -1
|
||||
#p.createVisualShape(p.GEOM_MESH,
|
||||
# fileName=filename,
|
||||
# meshScale=scaling)
|
||||
|
||||
# texture_filename = os.path.join(get_model_path(self.model_id), "{}_mesh_texture.small.jpg".format(self.model_id))
|
||||
# texture_id = p.loadTexture(texture_filename)
|
||||
# print('pybullet texture id:', texture_id, texture_filename)
|
||||
|
||||
boundaryUid = p.createMultiBody(baseCollisionShapeIndex=collisionId,
|
||||
baseVisualShapeIndex=visualId)
|
||||
|
||||
self.mesh_body_id = boundaryUid
|
||||
p.changeDynamics(boundaryUid, -1, lateralFriction=1)
|
||||
|
||||
planeName = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
|
||||
|
|
@ -137,14 +182,18 @@ class BuildingScene(Scene):
|
|||
p.resetBasePositionAndOrientation(self.ground_plane_mjcf[0],
|
||||
posObj=[0, 0, 0],
|
||||
ornObj=[0, 0, 0, 1])
|
||||
p.changeVisualShape(boundaryUid,
|
||||
-1,
|
||||
rgbaColor=[168 / 255.0, 164 / 255.0, 92 / 255.0, 1.0],
|
||||
specularColor=[0.5, 0.5, 0.5])
|
||||
# p.changeVisualShape(boundaryUid,
|
||||
# -1,
|
||||
# rgbaColor=[168 / 255.0, 164 / 255.0, 92 / 255.0, 1.0],
|
||||
# specularColor=[0.5, 0.5, 0.5])
|
||||
# if texture_id >= 0:
|
||||
# p.changeVisualShape(boundaryUid,
|
||||
# -1,
|
||||
# textureUniqueId=texture_id)
|
||||
|
||||
p.changeVisualShape(self.ground_plane_mjcf[0],
|
||||
-1,
|
||||
rgbaColor=[168 / 255.0, 164 / 255.0, 92 / 255.0, 1.0],
|
||||
rgbaColor=[168 / 255.0, 164 / 255.0, 92 / 255.0, 0.35],
|
||||
specularColor=[0.5, 0.5, 0.5])
|
||||
|
||||
floor_height_path = os.path.join(get_model_path(self.model_id), 'floors.txt')
|
||||
|
|
@ -156,39 +205,54 @@ class BuildingScene(Scene):
|
|||
self.floors = sorted(list(map(float, f.readlines())))
|
||||
print('floors', self.floors)
|
||||
for f in range(len(self.floors)):
|
||||
trav_map = Image.open(os.path.join(get_model_path(self.model_id), 'floor_trav_{}.png'.format(f)))
|
||||
obstacle_map = Image.open(os.path.join(get_model_path(self.model_id), 'floor_{}.png'.format(f)))
|
||||
trav_map = np.array(Image.open(
|
||||
os.path.join(get_model_path(self.model_id), 'floor_trav_{}.png'.format(f))
|
||||
))
|
||||
obstacle_map = np.array(Image.open(
|
||||
os.path.join(get_model_path(self.model_id), 'floor_{}.png'.format(f))
|
||||
))
|
||||
if self.trav_map_original_size is None:
|
||||
width, height = trav_map.size
|
||||
assert width == height, 'trav map is not a square'
|
||||
height, width = trav_map.shape
|
||||
assert height == width, 'trav map is not a square'
|
||||
self.trav_map_original_size = height
|
||||
self.trav_map_size = int(self.trav_map_original_size * self.trav_map_default_resolution / self.trav_map_resolution)
|
||||
trav_map = np.array(trav_map.resize((self.trav_map_size, self.trav_map_size)))
|
||||
obstacle_map = np.array(obstacle_map.resize((self.trav_map_size, self.trav_map_size)))
|
||||
self.trav_map_size = int(self.trav_map_original_size *
|
||||
self.trav_map_default_resolution /
|
||||
self.trav_map_resolution)
|
||||
trav_map[obstacle_map == 0] = 0
|
||||
trav_map = cv2.resize(trav_map, (self.trav_map_size, self.trav_map_size))
|
||||
trav_map = cv2.erode(trav_map, np.ones((self.trav_map_erosion, self.trav_map_erosion)))
|
||||
trav_map[trav_map < 255] = 0
|
||||
|
||||
if self.build_graph:
|
||||
g = nx.Graph()
|
||||
for i in range(self.trav_map_size):
|
||||
for j in range(self.trav_map_size):
|
||||
if trav_map[i, j] > 0:
|
||||
g.add_node((i, j))
|
||||
# 8-connected graph
|
||||
neighbors = [(i - 1, j - 1), (i, j - 1), (i + 1, j - 1), (i - 1, j)]
|
||||
for n in neighbors:
|
||||
if 0 <= n[0] < self.trav_map_size and 0 <= n[1] < self.trav_map_size and \
|
||||
trav_map[n[0], n[1]] > 0:
|
||||
g.add_edge(n, (i, j), weight=self.l2_distance(n, (i, j)))
|
||||
graph_file = os.path.join(get_model_path(self.model_id), 'floor_trav_{}.p'.format(f))
|
||||
if os.path.isfile(graph_file):
|
||||
print("load traversable graph")
|
||||
with open(graph_file, 'rb') as pfile:
|
||||
g = pickle.load(pfile)
|
||||
else:
|
||||
print("build traversable graph")
|
||||
g = nx.Graph()
|
||||
for i in range(self.trav_map_size):
|
||||
for j in range(self.trav_map_size):
|
||||
if trav_map[i, j] > 0:
|
||||
g.add_node((i, j))
|
||||
# 8-connected graph
|
||||
neighbors = [(i - 1, j - 1), (i, j - 1), (i + 1, j - 1), (i - 1, j)]
|
||||
for n in neighbors:
|
||||
if 0 <= n[0] < self.trav_map_size and 0 <= n[1] < self.trav_map_size and \
|
||||
trav_map[n[0], n[1]] > 0:
|
||||
g.add_edge(n, (i, j), weight=self.l2_distance(n, (i, j)))
|
||||
|
||||
# only take the largest connected component
|
||||
largest_cc = max(nx.connected_components(g), key=len)
|
||||
g = g.subgraph(largest_cc).copy()
|
||||
with open(graph_file, 'wb') as pfile:
|
||||
pickle.dump(g, pfile, protocol=pickle.HIGHEST_PROTOCOL)
|
||||
|
||||
# only take the largest connected component
|
||||
largest_cc = max(nx.connected_components(g), key=len)
|
||||
g = g.subgraph(largest_cc).copy()
|
||||
self.floor_graph.append(g)
|
||||
|
||||
# update trav_map accordingly
|
||||
trav_map[:, :] = 0
|
||||
for node in largest_cc:
|
||||
for node in g.nodes:
|
||||
trav_map[node[0], node[1]] = 255
|
||||
|
||||
self.floor_map.append(trav_map)
|
||||
|
|
@ -220,6 +284,11 @@ class BuildingScene(Scene):
|
|||
def world_to_map(self, xy):
|
||||
return np.flip((xy / self.trav_map_resolution + self.trav_map_size / 2.0)).astype(np.int)
|
||||
|
||||
def has_node(self, floor, world_xy):
|
||||
map_xy = tuple(self.world_to_map(world_xy))
|
||||
g = self.floor_graph[floor]
|
||||
return g.has_node(map_xy)
|
||||
|
||||
def get_shortest_path(self, floor, source_world, target_world, entire_path=False):
|
||||
# print("called shortest path", source_world, target_world)
|
||||
assert self.build_graph, 'cannot get shortest path without building the graph'
|
||||
|
|
@ -242,13 +311,15 @@ class BuildingScene(Scene):
|
|||
|
||||
path_world = self.map_to_world(path_map)
|
||||
geodesic_distance = np.sum(np.linalg.norm(path_world[1:] - path_world[:-1], axis=1))
|
||||
path_world = path_world[::self.waypoint_interval]
|
||||
|
||||
if not entire_path:
|
||||
path_world = path_world[::self.waypoint_interval][:self.num_waypoints]
|
||||
path_world = path_world[:self.num_waypoints]
|
||||
num_remaining_waypoints = self.num_waypoints - path_world.shape[0]
|
||||
if num_remaining_waypoints > 0:
|
||||
remaining_waypoints = np.tile(target_world, (num_remaining_waypoints, 1))
|
||||
path_world = np.concatenate((path_world, remaining_waypoints), axis=0)
|
||||
|
||||
return path_world, geodesic_distance
|
||||
|
||||
def reset_floor(self, floor=0, additional_elevation=0.05, height=None):
|
||||
|
|
|
|||
|
|
@ -100,6 +100,15 @@ class Simulator:
|
|||
self.scene = None
|
||||
self.objects = []
|
||||
|
||||
def load_without_pybullet_vis(load_func):
|
||||
def wrapped_load_func(*args, **kwargs):
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, False)
|
||||
res = load_func(*args, **kwargs)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, True)
|
||||
return res
|
||||
return wrapped_load_func
|
||||
|
||||
@load_without_pybullet_vis
|
||||
def import_scene(self, scene, texture_scale=1.0, load_texture=True, class_id=0):
|
||||
|
||||
"""
|
||||
|
|
@ -147,9 +156,12 @@ class Simulator:
|
|||
# class_id=class_id,
|
||||
# dynamic=True)
|
||||
|
||||
# Uncomment the above block if you want to render the additionally added floor
|
||||
pass
|
||||
self.scene = scene
|
||||
return new_objects
|
||||
|
||||
@load_without_pybullet_vis
|
||||
def import_object(self, object, class_id=0):
|
||||
"""
|
||||
:param object: Object to load
|
||||
|
|
@ -215,6 +227,7 @@ class Simulator:
|
|||
|
||||
return new_object
|
||||
|
||||
@load_without_pybullet_vis
|
||||
def import_robot(self, robot, class_id=0):
|
||||
"""
|
||||
Import a robot into Simulator
|
||||
|
|
@ -298,6 +311,7 @@ class Simulator:
|
|||
|
||||
return ids
|
||||
|
||||
@load_without_pybullet_vis
|
||||
def import_interactive_object(self, obj, class_id=0):
|
||||
"""
|
||||
Import articulated objects into simulator
|
||||
|
|
@ -387,6 +401,12 @@ class Simulator:
|
|||
"""
|
||||
|
||||
p.stepSimulation()
|
||||
self.sync()
|
||||
|
||||
def sync(self):
|
||||
"""
|
||||
Update positions in renderer without stepping the simulation. Usually used in the reset() function
|
||||
"""
|
||||
for instance in self.renderer.instances:
|
||||
if instance.dynamic:
|
||||
self.update_position(instance)
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
from gibson2.core.physics.robot_locomotors import Turtlebot, Husky, Ant, Humanoid, JR2, JR2_Kinova, Freight, Fetch
|
||||
from gibson2.core.physics.robot_locomotors import Turtlebot, Husky, Ant, Humanoid, JR2, JR2_Kinova, Freight, Fetch, \
|
||||
Locobot
|
||||
from gibson2.core.simulator import Simulator
|
||||
from gibson2.core.physics.scene import BuildingScene, StadiumScene
|
||||
import gibson2
|
||||
|
|
@ -52,6 +53,8 @@ class BaseEnv(gym.Env):
|
|||
elif self.config['scene'] == 'building':
|
||||
scene = BuildingScene(
|
||||
self.config['model_id'],
|
||||
waypoint_resolution=self.config.get('waypoint_resolution', 0.2),
|
||||
num_waypoints=self.config.get('num_waypoints', 10),
|
||||
build_graph=self.config.get('build_graph', False),
|
||||
trav_map_erosion=self.config.get('trav_map_erosion', 2),
|
||||
should_load_replaced_objects=self.config.get('should_load_replaced_objects', False)
|
||||
|
|
@ -77,6 +80,8 @@ class BaseEnv(gym.Env):
|
|||
robot = Freight(self.config)
|
||||
elif self.config['robot'] == 'Fetch':
|
||||
robot = Fetch(self.config)
|
||||
elif self.config['robot'] == 'Locobot':
|
||||
robot = Locobot(self.config)
|
||||
else:
|
||||
raise Exception('unknown robot type: {}'.format(self.config['robot']))
|
||||
|
||||
|
|
@ -105,4 +110,4 @@ class BaseEnv(gym.Env):
|
|||
return NotImplementedError
|
||||
|
||||
def set_mode(self, mode):
|
||||
self.simulator.mode = mode
|
||||
self.simulator.mode = mode
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -21,6 +21,7 @@ directory = os.path.dirname(os.path.abspath(__file__))
|
|||
sys.path.append(os.path.join(directory, '../motion'))
|
||||
from motion_planners.rrt_connect import birrt, direct_path
|
||||
#from ..motion.motion_planners.rrt_connect import birrt, direct_path
|
||||
import cv2
|
||||
|
||||
# from future_builtins import map, filter
|
||||
# from builtins import input # TODO - use future
|
||||
|
|
@ -972,7 +973,7 @@ def base_values_from_pose(pose, tolerance=1e-3):
|
|||
(point, quat) = pose
|
||||
x, y, _ = point
|
||||
roll, pitch, yaw = euler_from_quat(quat)
|
||||
assert (abs(roll) < tolerance) and (abs(pitch) < tolerance)
|
||||
#assert (abs(roll) < tolerance) and (abs(pitch) < tolerance)
|
||||
return (x, y, yaw)
|
||||
|
||||
pose2d_from_pose = base_values_from_pose
|
||||
|
|
@ -1082,6 +1083,11 @@ def set_base_values(body, values):
|
|||
set_point(body, (x, y, z))
|
||||
set_quat(body, z_rotation(theta))
|
||||
|
||||
def set_base_values_with_z(body, values, z):
|
||||
x, y, theta = values
|
||||
set_point(body, (x, y, z))
|
||||
set_quat(body, z_rotation(theta))
|
||||
|
||||
def get_velocity(body):
|
||||
linear, angular = p.getBaseVelocity(body, physicsClientId=CLIENT)
|
||||
return linear, angular # [x,y,z], [wx,wy,wz]
|
||||
|
|
@ -2450,7 +2456,8 @@ def get_collision_fn(body, joints, obstacles, attachments, self_collisions, disa
|
|||
# TODO: convert most of these to keyword arguments
|
||||
check_link_pairs = get_self_link_pairs(body, joints, disabled_collisions) \
|
||||
if self_collisions else []
|
||||
moving_links = frozenset(get_moving_links(body, joints))
|
||||
moving_links = frozenset([item for item in get_moving_links(body, joints) if item != 19])
|
||||
# TODO: This is a fetch specific change
|
||||
attached_bodies = [attachment.child for attachment in attachments]
|
||||
moving_bodies = [(body, moving_links)] + attached_bodies
|
||||
#moving_bodies = [body] + [attachment.child for attachment in attachments]
|
||||
|
|
@ -2461,8 +2468,10 @@ def get_collision_fn(body, joints, obstacles, attachments, self_collisions, disa
|
|||
# TODO: test self collision with the holding
|
||||
def collision_fn(q):
|
||||
if not all_between(lower_limits, q, upper_limits):
|
||||
#print('Joint limits violated')
|
||||
return True
|
||||
pass
|
||||
# print(lower_limits, q, upper_limits)
|
||||
# print('Joint limits violated')
|
||||
#return True
|
||||
set_joint_positions(body, joints, q)
|
||||
for attachment in attachments:
|
||||
attachment.assign()
|
||||
|
|
@ -2473,6 +2482,7 @@ def get_collision_fn(body, joints, obstacles, attachments, self_collisions, disa
|
|||
return True
|
||||
for body1, body2 in check_body_pairs:
|
||||
if pairwise_collision(body1, body2, **kwargs):
|
||||
#print(body1, body2)
|
||||
#print(get_body_name(body1), get_body_name(body2))
|
||||
return True
|
||||
return False
|
||||
|
|
@ -2507,10 +2517,10 @@ def plan_direct_joint_motion(body, joints, end_conf, **kwargs):
|
|||
|
||||
def check_initial_end(start_conf, end_conf, collision_fn):
|
||||
if collision_fn(start_conf):
|
||||
print("Warning: initial configuration is in collision")
|
||||
#print("Warning: initial configuration is in collision")
|
||||
return False
|
||||
if collision_fn(end_conf):
|
||||
print("Warning: end configuration is in collision")
|
||||
#print("Warning: end configuration is in collision")
|
||||
return False
|
||||
return True
|
||||
|
||||
|
|
@ -2652,13 +2662,27 @@ def plan_base_motion(body, end_conf, base_limits, obstacles=[], direct=False,
|
|||
distance_fn = get_base_distance_fn(weights=weights)
|
||||
|
||||
def extend_fn(q1, q2):
|
||||
steps = np.abs(np.divide(difference_fn(q2, q1), resolutions))
|
||||
n = int(np.max(steps)) + 1
|
||||
q = q1
|
||||
for i in range(n):
|
||||
q = tuple((1. / (n - i)) * np.array(difference_fn(q2, q)) + q)
|
||||
target_theta = np.arctan2(q2[1]-q1[1], q2[0]-q1[0])
|
||||
|
||||
n1 = int(np.abs(circular_difference(target_theta, q1[2]) / resolutions[2])) + 1
|
||||
n3 = int(np.abs(circular_difference(q2[2], target_theta) / resolutions[2])) + 1
|
||||
steps2 = np.abs(np.divide(difference_fn(q2, q1), resolutions))
|
||||
n2 = int(np.max(steps2)) + 1
|
||||
|
||||
for i in range(n1):
|
||||
q = (i / (n1)) * np.array(difference_fn((q1[0], q1[1], target_theta), q1)) + np.array(q1)
|
||||
q = tuple(q)
|
||||
yield q
|
||||
|
||||
for i in range(n2):
|
||||
q = (i / (n2)) * np.array(difference_fn((q2[0], q2[1], target_theta), (q1[0], q1[1], target_theta))) + np.array((q1[0], q1[1], target_theta))
|
||||
q = tuple(q)
|
||||
yield q
|
||||
|
||||
for i in range(n3):
|
||||
q = (i / (n3)) * np.array(difference_fn(q2, (q2[0], q2[1], target_theta))) + np.array((q2[0], q2[1], target_theta))
|
||||
q = tuple(q)
|
||||
yield q
|
||||
# TODO: should wrap these joints
|
||||
|
||||
def collision_fn(q):
|
||||
# TODO: update this function
|
||||
|
|
@ -2667,16 +2691,101 @@ def plan_base_motion(body, end_conf, base_limits, obstacles=[], direct=False,
|
|||
|
||||
start_conf = get_base_values(body)
|
||||
if collision_fn(start_conf):
|
||||
print("Warning: initial configuration is in collision")
|
||||
#print("Warning: initial configuration is in collision")
|
||||
return None
|
||||
if collision_fn(end_conf):
|
||||
print("Warning: end configuration is in collision")
|
||||
#print("Warning: end configuration is in collision")
|
||||
return None
|
||||
if direct:
|
||||
return direct_path(start_conf, end_conf, extend_fn, collision_fn)
|
||||
return birrt(start_conf, end_conf, distance_fn,
|
||||
sample_fn, extend_fn, collision_fn, **kwargs)
|
||||
|
||||
|
||||
def plan_base_motion_2d(body, end_conf, base_limits, map_2d, occupancy_range, grid_resolution, robot_footprint_radius_in_map,
|
||||
obstacles=[], direct=False, weights=1 * np.ones(3), resolutions=0.05 * np.ones(3),
|
||||
max_distance=MAX_DISTANCE, min_goal_dist = 0.02, **kwargs):
|
||||
def sample_fn():
|
||||
x, y = np.random.uniform(*base_limits)
|
||||
theta = np.random.uniform(*CIRCULAR_LIMITS)
|
||||
return (x, y, theta)
|
||||
|
||||
difference_fn = get_base_difference_fn()
|
||||
distance_fn = get_base_distance_fn(weights=weights)
|
||||
|
||||
def extend_fn(q1, q2):
|
||||
target_theta = np.arctan2(q2[1] - q1[1], q2[0] - q1[0])
|
||||
|
||||
n1 = int(np.abs(circular_difference(target_theta, q1[2]) / resolutions[2])) + 1
|
||||
n3 = int(np.abs(circular_difference(q2[2], target_theta) / resolutions[2])) + 1
|
||||
steps2 = np.abs(np.divide(difference_fn(q2, q1), resolutions))
|
||||
n2 = int(np.max(steps2)) + 1
|
||||
|
||||
for i in range(n1):
|
||||
q = (i / (n1)) * np.array(difference_fn((q1[0], q1[1], target_theta), q1)) + np.array(q1)
|
||||
q = tuple(q)
|
||||
yield q
|
||||
|
||||
for i in range(n2):
|
||||
q = (i / (n2)) * np.array(
|
||||
difference_fn((q2[0], q2[1], target_theta), (q1[0], q1[1], target_theta))) + np.array(
|
||||
(q1[0], q1[1], target_theta))
|
||||
q = tuple(q)
|
||||
yield q
|
||||
|
||||
for i in range(n3):
|
||||
q = (i / (n3)) * np.array(difference_fn(q2, (q2[0], q2[1], target_theta))) + np.array(
|
||||
(q2[0], q2[1], target_theta))
|
||||
q = tuple(q)
|
||||
yield q
|
||||
|
||||
start_conf = get_base_values(body)
|
||||
|
||||
if np.abs(start_conf[0] - end_conf[0]) < min_goal_dist and np.abs(start_conf[1] - end_conf[1]) < min_goal_dist:
|
||||
# do not do plans that is smaller than 30mm
|
||||
return None
|
||||
|
||||
def collision_fn(q):
|
||||
# TODO: update this function
|
||||
# set_base_values(body, q)
|
||||
# return any(pairwise_collision(body, obs, max_distance=max_distance) for obs in obstacles)
|
||||
delta = np.array(q)[:2] - np.array(start_conf)[:2]
|
||||
theta = start_conf[2]
|
||||
x_dir = np.array([np.sin(theta), -np.cos(theta)])
|
||||
y_dir = np.array([np.cos(theta), np.sin(theta)])
|
||||
end_in_start_frame = [x_dir.dot(delta), y_dir.dot(delta)]
|
||||
pts = np.array(end_in_start_frame) / (occupancy_range / 2) * (grid_resolution / 2) + grid_resolution / 2
|
||||
pts = pts.astype(np.int32)
|
||||
#print(pts)
|
||||
|
||||
if pts[0] < robot_footprint_radius_in_map or pts[1] < robot_footprint_radius_in_map \
|
||||
or pts[0] > grid_resolution - robot_footprint_radius_in_map - 1 or pts[
|
||||
1] > grid_resolution - robot_footprint_radius_in_map - 1:
|
||||
return True
|
||||
|
||||
# plt.figure()
|
||||
# plt.imshow(map_2d[pts[0]-1:pts[0]+1, pts[1]-1:pts[1]+1])
|
||||
# plt.colorbar()
|
||||
mask = np.zeros((robot_footprint_radius_in_map * 2 + 1, robot_footprint_radius_in_map * 2 + 1))
|
||||
cv2.circle(mask, (robot_footprint_radius_in_map, robot_footprint_radius_in_map), robot_footprint_radius_in_map,
|
||||
1, -1)
|
||||
mask = mask.astype(np.bool)
|
||||
return np.any(map_2d[pts[0] - robot_footprint_radius_in_map:pts[0] + robot_footprint_radius_in_map + 1,
|
||||
pts[1] - robot_footprint_radius_in_map:pts[1] + robot_footprint_radius_in_map + 1][mask] == 0)
|
||||
|
||||
if collision_fn(start_conf):
|
||||
# print("Warning: initial configuration is in collision")
|
||||
return None
|
||||
if collision_fn(end_conf):
|
||||
# print("Warning: end configuration is in collision")
|
||||
return None
|
||||
if direct:
|
||||
return direct_path(start_conf, end_conf, extend_fn, collision_fn)
|
||||
return birrt(start_conf, end_conf, distance_fn,
|
||||
sample_fn, extend_fn, collision_fn, **kwargs)
|
||||
|
||||
|
||||
|
||||
#####################################
|
||||
|
||||
# Placements
|
||||
|
|
@ -2773,6 +2882,34 @@ def get_constraints():
|
|||
return [p.getConstraintUniqueId(i, physicsClientId=CLIENT)
|
||||
for i in range(p.getNumConstraints(physicsClientId=CLIENT))]
|
||||
|
||||
def add_p2p_constraint(body, body_link, robot, robot_link, max_force=None):
|
||||
if body_link == -1:
|
||||
body_pose = get_pose(body)
|
||||
else:
|
||||
body_pose = get_com_pose(body, link=body_link)
|
||||
#end_effector_pose = get_link_pose(robot, robot_link)
|
||||
end_effector_pose = get_com_pose(robot, robot_link)
|
||||
grasp_pose = multiply(invert(end_effector_pose), body_pose)
|
||||
point, quat = grasp_pose
|
||||
# TODO: can I do this when I'm not adjacent?
|
||||
# joint axis in local frame (ignored for JOINT_FIXED)
|
||||
#return p.createConstraint(robot, robot_link, body, body_link,
|
||||
# p.JOINT_FIXED, jointAxis=unit_point(),
|
||||
# parentFramePosition=unit_point(),
|
||||
# childFramePosition=point,
|
||||
# parentFrameOrientation=unit_quat(),
|
||||
# childFrameOrientation=quat)
|
||||
constraint = p.createConstraint(robot, robot_link, body, body_link, # Both seem to work
|
||||
p.JOINT_POINT2POINT, jointAxis=unit_point(),
|
||||
parentFramePosition=point,
|
||||
childFramePosition=unit_point(),
|
||||
parentFrameOrientation=quat,
|
||||
childFrameOrientation=unit_quat(),
|
||||
physicsClientId=CLIENT)
|
||||
if max_force is not None:
|
||||
p.changeConstraint(constraint, maxForce=max_force, physicsClientId=CLIENT)
|
||||
return constraint
|
||||
|
||||
def remove_constraint(constraint):
|
||||
p.removeConstraint(constraint, physicsClientId=CLIENT)
|
||||
|
||||
|
|
@ -2912,13 +3049,13 @@ def control_joints(body, joints, positions):
|
|||
# TODO: the whole PR2 seems to jitter
|
||||
#kp = 1.0
|
||||
#kv = 0.3
|
||||
#forces = [get_max_force(body, joint) for joint in joints]
|
||||
forces = [get_max_force(body, joint) * 100 for joint in joints]
|
||||
#forces = [5000]*len(joints)
|
||||
#forces = [20000]*len(joints)
|
||||
return p.setJointMotorControlArray(body, joints, p.POSITION_CONTROL,
|
||||
targetPositions=positions,
|
||||
targetVelocities=[0.0] * len(joints),
|
||||
physicsClientId=CLIENT) #,
|
||||
physicsClientId=CLIENT, forces=forces) #,
|
||||
#positionGains=[kp] * len(joints),
|
||||
#velocityGains=[kv] * len(joints),)
|
||||
#forces=forces)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
import numpy as np
|
||||
import yaml
|
||||
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
|
||||
# File I/O related
|
||||
def parse_config(config):
|
||||
|
|
@ -11,11 +11,22 @@ def parse_config(config):
|
|||
|
||||
# Geometry related
|
||||
def rotate_vector_3d(v, r, p, y):
|
||||
"""Rotates vector by roll, pitch and yaw counterclockwise"""
|
||||
rot_x = np.array([[1, 0, 0], [0, np.cos(-r), -np.sin(-r)], [0, np.sin(-r), np.cos(-r)]])
|
||||
rot_y = np.array([[np.cos(-p), 0, np.sin(-p)], [0, 1, 0], [-np.sin(-p), 0, np.cos(-p)]])
|
||||
rot_z = np.array([[np.cos(-y), -np.sin(-y), 0], [np.sin(-y), np.cos(-y), 0], [0, 0, 1]])
|
||||
return np.dot(rot_x, np.dot(rot_y, np.dot(rot_z, v)))
|
||||
local_to_global = R.from_euler('xyz', [r, p, y]).as_dcm()
|
||||
global_to_local = local_to_global.T
|
||||
return np.dot(global_to_local, v)
|
||||
|
||||
|
||||
def rotate_vector_2d(v, yaw):
|
||||
local_to_global = R.from_euler('z', yaw).as_dcm()
|
||||
global_to_local = local_to_global.T
|
||||
global_to_local = global_to_local[:2, :2]
|
||||
if len(v.shape) == 1:
|
||||
return np.dot(global_to_local, v)
|
||||
elif len(v.shape) == 2:
|
||||
return np.dot(global_to_local, v.T).T
|
||||
else:
|
||||
print(v.shape)
|
||||
raise Exception('invalid shape for v')
|
||||
|
||||
|
||||
def l2_distance(v1, v2):
|
||||
|
|
|
|||
Loading…
Reference in New Issue