# Untitled

a guest
Oct 23rd, 2019
101
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
1. import numpy as np
2. from pydrake.multibody import inverse_kinematics
3. from pydrake.trajectories import PiecewisePolynomial
4.
5. from pydrake.math import RollPitchYaw, RotationMatrix, RigidTransform
6. from pydrake.examples.manipulation_station import ManipulationStation
7. import pydrake.solvers.mathematicalprogram as mp
8. from manip_station_sim.plan_utils import ConnectPointsWithCubicPolynomial
9.
10. # starting position of point Q in world frame.
11. p_WQ_home = np.array([0.5, 0, 0.4])
12.
13. # position of point Q in EE frame. Point Q is fixed in the EE frame.
14. p_L7Q = np.array([0., 0., 0.21])
15.
16. # robot home position
17. q_home = np.array([0, 0, 0, -1.75, 0, 1.0, 0])
18.
19.
20. def GenerateIiwaTrajectoriesAndGripperSetPoints(X_WO, is_printing=True):
21. """
22. :param X_WO: the pose of the foam brick in world frame.
23. :param is_printing: set to True to print IK solution results.
24. :return: 1. a list of joint space trajectories.
25. 2. a list of gripper set points.
26. The two lists must have the same length.
27. """
28. station = ManipulationStation()
29. station.SetupManipulationClassStation()
30. station.Finalize()
31. plant = station.get_controller_plant()
32.
33. # Go to p_WQ_home
34. ik_iiwa = inverse_kinematics.InverseKinematics(plant)
35. world_frame = plant.world_frame()
37.
38. theta_bound = 0.005 * np.pi
39. R_WL7 = RollPitchYaw(0, np.pi * 5 / 6, 0).ToRotationMatrix()
40.
42. frameAbar=world_frame, R_AbarA=R_WL7,
43. frameBbar=l7_frame, R_BbarB=RotationMatrix(),
44. theta_bound=theta_bound)
45.
47. frameB=l7_frame, p_BQ=p_L7Q,
48. frameA=world_frame,
49. p_AQ_lower=p_WQ_home - 0.001,
50. p_AQ_upper=p_WQ_home + 0.001)
51.
52. prog = ik_iiwa.prog()
53. prog.SetInitialGuess(ik_iiwa.q(), np.zeros(7))
54. result = mp.Solve(prog)
55. if is_printing:
56. print(result.get_solution_result())
57. q_val_0 = result.GetSolution(ik_iiwa.q())
58.
59. qtraj_leave_home = ConnectPointsWithCubicPolynomial(q_home, q_val_0, 4.0)
60.
61. # open fingers
62. q_knots = np.zeros((2, 7))
63. q_knots[0] = qtraj_leave_home.value(qtraj_leave_home.end_time()).squeeze()
64. q_knots[1] = q_knots[0]
65. qtraj_open_gripper = PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots.T)
66.
67. ik_iiwa = inverse_kinematics.InverseKinematics(plant)
69. frameAbar=world_frame, R_AbarA=R_WL7,
70. frameBbar=l7_frame, R_BbarB=RotationMatrix(),
71. theta_bound=theta_bound)
72.
74. frameB=l7_frame, p_BQ=p_L7Q,
75. frameA=world_frame,
76. p_AQ_lower=X_WO.translation() + np.array([0.0, -0.01, 0.0]) - 0.0001,
77. p_AQ_upper=X_WO.translation() + np.array([0.0, -0.01, 0.0]) + 0.0001)
78.
79. prog = ik_iiwa.prog()
80. prog.SetInitialGuess(ik_iiwa.q(), np.zeros(7))
81. result = mp.Solve(prog)
82. if is_printing:
83. print(result.get_solution_result())
84. q_val_1 = result.GetSolution(ik_iiwa.q())
85.
86. qtraj_reach_foam = ConnectPointsWithCubicPolynomial(q_val_0, q_val_1, 6.0)
87.
88. # close fingers
89. q_knots = np.zeros((2, 7))
90. q_knots[0] = qtraj_reach_foam.value(qtraj_reach_foam.end_time()).squeeze()
91. q_knots[1] = q_knots[0]
92. qtraj_close_gripper = PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots.T)
93.
94. qtraj_return_home = ConnectPointsWithCubicPolynomial(q_val_1, q_val_0, 6.0)
95.
96. R_WL8 = RollPitchYaw(0, np.pi * 3 / 6, 0).ToRotationMatrix()
97.
98. ik_iiwa = inverse_kinematics.InverseKinematics(plant)
100. frameAbar=world_frame, R_AbarA=R_WL8,
101. frameBbar=l7_frame, R_BbarB=RotationMatrix(),
102. theta_bound=theta_bound)
103.
105. frameB=l7_frame, p_BQ=p_L7Q,
106. frameA=world_frame,
107. p_AQ_lower=p_WQ_home + np.array([0.4, 0.0, 0.0]) - 0.0001,
108. p_AQ_upper=p_WQ_home + np.array([0.4, 0.0, 0.0]) + 0.0001)
109.
110. prog = ik_iiwa.prog()
111. prog.SetInitialGuess(ik_iiwa.q(), np.zeros(7))
112. result = mp.Solve(prog)
113. if is_printing:
114. print(result.get_solution_result())
115. q_val_2 = result.GetSolution(ik_iiwa.q())
116.
117. qtraj_reach_shelf = ConnectPointsWithCubicPolynomial(q_val_0, q_val_2, 6.0)
118.
119. # open fingers
120. q_knots = np.zeros((2, 7))
121. q_knots[0] = qtraj_reach_shelf.value(qtraj_reach_shelf.end_time()).squeeze()
122. q_knots[1] = q_knots[0]
123. qtraj_open_gripper2 = PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots.T)
124.
125. qtraj_return_home2 = ConnectPointsWithCubicPolynomial(q_val_2, q_val_0, 6.0)
126. qtraj_return_home3 = ConnectPointsWithCubicPolynomial(q_val_0, q_home, 4.0)
127.
128. # Complete your pick and place trajectories.
129.
130. q_traj_list = [qtraj_leave_home,
131. qtraj_open_gripper,
132. qtraj_reach_foam,
133. qtraj_close_gripper,
134. qtraj_return_home,
135. qtraj_reach_shelf,
136. qtraj_open_gripper2,
137. qtraj_return_home2,
138. qtraj_return_home3,
139. ]
140. gripper_setpoint_list = [0.01, 0.1, 0.1, 0.01, 0.01, 0.01, 0.1, 0.1, 0.1]
141.
142. return q_traj_list, gripper_setpoint_list
RAW Paste Data