Guest User

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()
  36. l7_frame = plant.GetFrameByName("iiwa_link_7")
  37.  
  38. theta_bound = 0.005 * np.pi
  39. R_WL7 = RollPitchYaw(0, np.pi * 5 / 6, 0).ToRotationMatrix()
  40.  
  41. ik_iiwa.AddOrientationConstraint(
  42. frameAbar=world_frame, R_AbarA=R_WL7,
  43. frameBbar=l7_frame, R_BbarB=RotationMatrix(),
  44. theta_bound=theta_bound)
  45.  
  46. ik_iiwa.AddPositionConstraint(
  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)
  68. ik_iiwa.AddOrientationConstraint(
  69. frameAbar=world_frame, R_AbarA=R_WL7,
  70. frameBbar=l7_frame, R_BbarB=RotationMatrix(),
  71. theta_bound=theta_bound)
  72.  
  73. ik_iiwa.AddPositionConstraint(
  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)
  99. ik_iiwa.AddOrientationConstraint(
  100. frameAbar=world_frame, R_AbarA=R_WL8,
  101. frameBbar=l7_frame, R_BbarB=RotationMatrix(),
  102. theta_bound=theta_bound)
  103.  
  104. ik_iiwa.AddPositionConstraint(
  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