SHARE
TWEET

Untitled

a guest Oct 23rd, 2019 87 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
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top