-
Notifications
You must be signed in to change notification settings - Fork 0
/
optimization_objective.py
116 lines (97 loc) · 6.11 KB
/
optimization_objective.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
class OptimizationObjective(object):
def __init__(self, tau, C, V, S, ry):
"""
store references to global configuration, visualization, simulation and library
initialize new komo one-step path
tau: simulation time step in seconds
C: configuration
V: visualization
S: simulation
ry: library imported elsewhere (may not want to import twice)
"""
self.tau = tau
self.C = C
self.V = V
self.S = S
self.ry = ry
self.komo = C.komo_path(1.0, 1, self.tau, True)
def clear(self):
"""
reset komo one-step path optimiation objectives
"""
# TODO: reinitialization may not be necessary if resetting anyways
self.komo = self.C.komo_path(1.0, 1, self.tau, True)
self.komo.clearObjectives()
self.komo.add_qControlObjective(order=1, scale=1)
def grasp(self, gripper, obj, align_vec_z = None, align_vec_y=None):
"""
objectives for gripping specified object with specified gripper
gripper: string which identifies the gripper ("R_gripper", "L_gripper")
obj: string which identifies the target object ("obj0")
align_vec_z: vector to align the zaxis of the gripper center with or None
align_vec_y: vector to align the yaxis of the gripper center with or None
"""
ry = self.ry
self.komo.addObjective([1.], ry.FS.positionDiff, [gripper+"Center",obj], ry.OT.sos, [2e3])
if align_vec_z:
self.komo.addObjective([1.], ry.FS.vectorZ, [gripper+"Center"], ry.OT.sos, [3e2], target=align_vec_z)
if align_vec_y:
self.komo.addObjective([1.], ry.FS.vectorY, [gripper+"Center"], ry.OT.sos, [3e2], target=align_vec_y)
self.komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.ineq, [1e2])
self.komo.addObjective([1.], ry.FS.qItself, [], ry.OT.sos, [2e1], order=1)
finger1 = "{}_finger1".format(gripper[0])
finger2 = "{}_finger2".format(gripper[0])
self.komo.addObjective([], ry.FS.qItself, [finger1], ry.OT.eq, [3e1], order=1)
self.komo.addObjective([], ry.FS.qItself, [finger2], ry.OT.eq, [3e1], order=1)
self.komo.addObjective([], ry.FS.positionDiff, [finger1, finger2], ry.OT.sos, [1e3], target=[0,0.1,0])
def move_to_position(self, gripper, pos, align_vec_z = None, align_vec_y=None, movement_priority=5e3, alignment_priority=3e3):
"""
objectives for moving the gripper center of specified gripper to specified position
gripper: string which identifies the gripper ("R_gripper", "L_gripper")
pos: position to align the gripper center with
align_vec_z: vector to align the zaxis of the gripper center with or None
align_vec_y: vector to align the yaxis of the gripper center with or None
movement_priority: priority of aligning the gripper position
alignment_priority: priority of aligning the gripper axes
"""
ry = self.ry
if align_vec_z:
self.komo.addObjective([1.], ry.FS.vectorZ, [gripper+"Center"], ry.OT.sos, [alignment_priority], target=align_vec_z)
if align_vec_y:
self.komo.addObjective([1.], ry.FS.vectorY, [gripper+"Center"], ry.OT.sos, [alignment_priority], target=align_vec_y)
self.komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.ineq, [1e3])
self.komo.addObjective([1.], ry.FS.position, [gripper + "Center"], ry.OT.sos, [movement_priority], target=pos)
self.komo.addObjective([0.,.1], ry.FS.qItself, [], ry.OT.sos, [5e3], order=1)
self.komo.addObjective([.9,1.], ry.FS.qItself, [], ry.OT.sos, [5e1], order=1)
self.komo.addObjective([], ry.FS.qItself, ["R_finger1"], ry.OT.eq, [1e2], order=1)
self.komo.addObjective([], ry.FS.qItself, ["R_finger2"], ry.OT.eq, [1e2], order=1)
self.komo.addObjective([], ry.FS.qItself, ["L_finger1"], ry.OT.eq, [1e2], order=1)
self.komo.addObjective([], ry.FS.qItself, ["L_finger2"], ry.OT.eq, [1e2], order=1)
def go_to_q(self, q):
"""
objectives to let the robot arms move to some state in joint space
q: the state in joint space to move to
"""
ry = self.ry
self.komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.ineq, [1e3])
self.komo.addObjective([1.], ry.FS.qItself, [], ry.OT.sos, [5e2], target=q)
self.komo.addObjective([1.], ry.FS.qItself, [], ry.OT.sos, [2e1], order=1)
self.komo.addObjective([], ry.FS.qItself, ["R_finger1"], ry.OT.eq, [1e2], order=1)
self.komo.addObjective([], ry.FS.qItself, ["R_finger2"], ry.OT.eq, [1e2], order=1)
self.komo.addObjective([], ry.FS.qItself, ["L_finger1"], ry.OT.eq, [1e2], order=1)
self.komo.addObjective([], ry.FS.qItself, ["L_finger2"], ry.OT.eq, [1e2], order=1)
def go_to_handover(self, alignment_prio=2e2):
"""
objectives to let the robot arms do a handover
alignment_prio: priority of the objectives for aligning the gripper axes
"""
ry = self.ry
self.komo.addObjective([1.], ry.FS.positionDiff, ["R_gripperCenter","L_gripperCenter"], ry.OT.sos, [3e2])
self.komo.addObjective([1.], ry.FS.scalarProductZZ, ["R_gripperCenter","L_gripperCenter"], ry.OT.sos, [alignment_prio], target=[-1])
self.komo.addObjective([1.], ry.FS.scalarProductXX, ["R_gripperCenter","L_gripperCenter"], ry.OT.sos, [alignment_prio], target=[0])
self.komo.addObjective([], ry.FS.accumulatedCollisions, [], ry.OT.ineq, [1e2])
self.komo.addObjective([1.], ry.FS.qItself, [], ry.OT.sos, [1.5e1], order=1)
self.komo.addObjective([], ry.FS.qItself, ["R_finger1"], ry.OT.eq, [1e1], order=1)
self.komo.addObjective([], ry.FS.qItself, ["L_finger1"], ry.OT.eq, [1e1], order=1)
self.komo.addObjective([], ry.FS.qItself, ["R_finger2"], ry.OT.eq, [1e1], order=1)
self.komo.addObjective([], ry.FS.qItself, ["L_finger2"], ry.OT.eq, [1e1], order=1)