-
Notifications
You must be signed in to change notification settings - Fork 0
/
exp_utils.py
77 lines (63 loc) · 2.74 KB
/
exp_utils.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
"""
Copyright (c) 2022 Alvin Shek
This work is licensed under the terms of the MIT license.
For a copy, see <https://opensource.org/licenses/MIT>.
"""
import numpy as np
import ipdb
from pybullet_tools.kuka_primitives import BodyConf, BodyPath, Command
from pybullet_tools.utils import *
def sigint_handler(signal, frame):
# Force scripts to exit cleanly
exit()
def plan_step(robot: int, tool_link: int, target_pose: Pose, obstacles: list,
custom_limits, ik_kwargs, grasp):
"""
Plan a low-level control trajectory to move the robot to the target pose.
:param robot: ID of the robot
:param tool_link: ID of the tool link
:param target_pose: Target pose
:param obstacles: List of IDs of the obstacles
:param custom_limits: Custom joint limits for the IK solver
:param ik_kwargs: Additional arguments for the IK solver
:param grasp: Grasp object
:return:
"""
# get current joint config of robot
current_conf = BodyConf(robot)
# calculate joint config of target pose
# target pose should be sufficiently close to current pose
q_approach = inverse_kinematics(robot, tool_link, target_pose,
max_iterations=500,
custom_limits=custom_limits,
**ik_kwargs)
if q_approach is None:
# Try one more time. This strangely always works after the first try
q_approach = inverse_kinematics(robot, tool_link, target_pose,
max_iterations=500,
custom_limits=custom_limits,
**ik_kwargs)
if q_approach is None:
raise Exception("IK Failed!")
if any(pairwise_collision(robot, b) for b in obstacles):
raise Exception("Collision!")
# plan_direct_joint_motion(robot idx, joint idxs, Pose, [obs], limits)
current_conf.assign()
path = plan_direct_joint_motion(robot, current_conf.joints, q_approach, obstacles=obstacles,
custom_limits=custom_limits, attachments=[grasp.attachment()],
disabled_collisions={grasp.body})
if path is None:
raise Exception("Joint Path Failed!")
# path = refine_path(robot, current_conf.joints, path, num_steps=10)
return Command([BodyPath(robot, path, attachments=[grasp])])
def load_floor():
floor_ids = []
dxs = [-2, 0, 2]
dys = [-1, 0, 1]
with HideOutput():
for dx in dxs:
for dy in dys:
floor = load_model('models/short_floor.urdf')
set_pose(floor, Pose(Point(dx, dy, 0), Euler(0, 0, 0)))
floor_ids.append(floor)
return floor_ids