-
Notifications
You must be signed in to change notification settings - Fork 3
/
Copy pathminjerk_w_id.py
148 lines (126 loc) · 4.82 KB
/
minjerk_w_id.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
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
import sys
import os
import time
import json
from functools import partial
import numpy as np
import pybullet as p
if os.environ.get('ROS_DISTRO'):
import rospy
from cairo_simulator.core.sim_context import SawyerSimContext
from cairo_simulator.core.simulator import SimObject
from cairo_simulator.core.primitives import create_box
from cairo_planning.core.planning_context import SawyerPlanningContext
from cairo_planning.collisions import DisabledCollisionsContext
from cairo_planning.local.interpolation import parametric_lerp
from cairo_planning.local.curve import JointTrajectoryCurve
from cairo_planning.planners import LazyPRM
from copy import copy
from ruckig import InputParameter, OutputParameter, Result, Ruckig, Synchronization, Interface, DurationDiscretization
import matplotlib.pyplot as plt
import numpy as np
import os
import sys
from functools import partial
import time
import pybullet as p
if os.environ.get('ROS_DISTRO'):
import rospy
import numpy as np
from cairo_simulator.core.sim_context import SawyerSimContext
from cairo_planning.collisions import DisabledCollisionsContext
from cairo_planning.local.interpolation import parametric_lerp
from cairo_planning.local.curve import JointTrajectoryCurve
from cairo_planning.planners import LazyPRM
def main():
limits = [['right_j0', (-3.0503, 3.0503)],
['right_j1', (-3.8095, 2.2736)],
['right_j2', (-3.0426, 3.0426)],
['right_j3', (-3.0439, 3.0439)],
['right_j4', (-2.9761, 2.9761)],
['right_j5', (-2.9761, 2.9761)],
['right_j6', (-4.7124, 4.7124)],
['right_gripper_l_finger_joint', (0.0, 0.020833)],
['right_gripper_r_finger_joint',
(-0.020833, 0.0)],
['head_pan', (-5.0952, 0.9064)]]
config = {}
config["sim_objects"] = [
{
"object_name": "Ground",
"model_file_or_sim_id": "plane.urdf",
"position": [0, 0, 0]
},
{
"object_name": "sphere",
"model_file_or_sim_id": 'sphere2.urdf',
"position": [1.0, -.3, .6],
"orientation": [0, 0, 1.5708],
"fixed_base": 1
}
# {
# "object_name": "Table",
# "model_file_or_sim_id": ASSETS_PATH + 'table.sdf',
# "position": [.8, -.6, .6],
# "orientation": [0, 0, 1.5708],
# "fixed_base": 1
# }
]
sim_context = SawyerSimContext(config)
sim = sim_context.get_sim_instance()
logger = sim_context.get_logger()
state_space = sim_context.get_state_space()
sawyer_robot = sim_context.get_robot()
svc = sim_context.get_state_validity()
interp_fn = partial(parametric_lerp, steps=20)
_ = sawyer_robot.get_simulator_id()
_ = sim_context.get_sim_objects(['Ground'])[0]
start = [0, 0, 0, 0, 0, 0, -np.pi/2]
goal = [-1.9622245072067646, 0.8439858364277937, 1.3628459180018329, -
0.2383928041974519, -2.7327884695211555, -2.2177502341009134, -0.08992133311928363]
####################################
# SIMULATION AND PLANNING CONTEXTS #
####################################
with DisabledCollisionsContext(sim, [], []):
#######
# PRM #
#######
# Use parametric linear interpolation with 10 steps between points.
interp = partial(parametric_lerp, steps=10)
# See params for PRM specific parameters
prm = LazyPRM(state_space, svc, interp_fn, params={
'n_samples': 5000, 'k': 8, 'ball_radius': 2.5})
logger.info("Planning....")
plan = prm.plan(np.array(start), np.array(goal))
# get_path() reuses the interp function to get the path between vertices of a successful plan
path = prm.get_path(plan)
if len(path) == 0:
logger.info("Planning failed....")
sys.exit(1)
logger.info("Plan found....")
# splinging uses numpy so needs to be converted
path = [np.array(p) for p in path]
logger.info("Length of path: {}".format(len(path)))
sawyer_robot.move_to_joint_pos(start)
time.sleep(3)
while sawyer_robot.check_if_at_position(start, 0.5) is False:
time.sleep(0.1)
sim.step()
if len(path) > 0:
# Create a MinJerk spline trajectory using JointTrajectoryCurve and execute
jtc = JointTrajectoryCurve()
traj = jtc.generate_trajectory(path, move_time=5)
traj_data = {}
traj_data["trajectory"] = []
for point in traj:
traj_point = {
"time": point[0],
"position": point[1],
"velocity": point[2],
"acceleration": point[3]
}
traj_data['trajectory'].append(traj_point)
with open('traj_w_id.json', 'w') as f:
json.dump(traj_data, f)
if __name__ == "__main__":
main()