final_plate_wipe.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "metadata": {
    "id": "eeMrMI0-1Dhu",
    "deepnote_to_be_reexecuted": false,
    "source_hash": "2b572daf",
    "execution_start": 1639025642656,
    "execution_millis": 916,
    "output_cleared": false,
    "cell_id": "00000-307e6998-f6b6-4ad6-8d3b-5453329710d7",
    "deepnote_cell_type": "code"
   },
   "source": "import os\nfrom functools import partial\n\nfrom IPython.display import display, clear_output, SVG\nimport mcubes\nimport numpy as np\nimport pydot\n\nimport pydrake\nfrom pydrake.all import (\n    Adder, AddMultibodyPlantSceneGraph, Box, Cylinder, DiagramBuilder, \n    InverseKinematics, MeshcatVisualizerCpp, MeshcatVisualizerParams,\n    Parser, FindResourceOrThrow, PassThrough,\n    RigidTransform, RollPitchYaw, RotationMatrix, Simulator, Solve, Sphere,\n    ConnectPlanarSceneGraphVisualizer, Demultiplexer, MultibodyPlant, PiecewiseQuaternionSlerp,\n    InverseDynamicsController, StateInterpolatorWithDiscreteDerivative, TrajectorySource,\n    LeafSystem, BasicVector, JacobianWrtVariable, RollPitchYaw, VectorLogSink, SpatialInertia, UnitInertia, Box, CoulombFriction,\n    MathematicalProgram, Solve, eq, le, ge\n)\nfrom pydrake.examples.manipulation_station import ManipulationStation\nfrom manipulation.meshcat_cpp_utils import (\n    StartMeshcat, MeshcatPoseSliders, AddMeshcatTriad\n)\nfrom manipulation.scenarios import (\n    AddIiwa, AddPlanarIiwa, AddTwoLinkIiwa, AddWsg, AddShape\n)\nfrom manipulation.utils import FindResource\nfrom manipulation import running_as_notebook\nfrom pydrake.trajectories import PiecewisePolynomial\nfrom pydrake.multibody import inverse_kinematics\nfrom pydrake.systems.controllers import PidController\nimport time\n# from scipy.spatial.distance import cdist",
   "outputs": [],
   "execution_count": 1
  },
  {
   "cell_type": "code",
   "metadata": {
    "deepnote_to_be_reexecuted": false,
    "source_hash": "6d42057f",
    "execution_start": 1639025643580,
    "execution_millis": 8,
    "output_cleared": false,
    "deepnote_output_heights": [
     20.734375,
     20.734375
    ],
    "cell_id": "00001-5c070224-a6a4-4d51-9a00-341c88a42c61",
    "deepnote_cell_type": "code"
   },
   "source": "# Start the visualizer.\nmeshcat = StartMeshcat()",
   "outputs": [
    {
     "data": {
      "text/plain": "<IPython.core.display.HTML object>",
      "text/html": "Meshcat is now available at <a href=\"https://e1f7a4de-0698-4427-bcc0-40d4d181e518.deepnoteproject.com\">https://e1f7a4de-0698-4427-bcc0-40d4d181e518.deepnoteproject.com</a>"
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "execution_count": 2
  },
  {
   "cell_type": "code",
   "metadata": {
    "tags": [],
    "cell_id": "00002-59fc08c5-69b9-4af6-820e-e29ff4c85ce6",
    "deepnote_to_be_reexecuted": false,
    "source_hash": "91773c86",
    "execution_start": 1639025643590,
    "execution_millis": 10,
    "deepnote_output_heights": [
     78.75
    ],
    "deepnote_cell_type": "code"
   },
   "source": "# Function to setup and solve TrajOpt problem to generate wiping locations on table\n\ndef create_optimized_wipe_traj(dirt_locs, start_pos, end_pos, n):\n    prog = MathematicalProgram()\n\n    # Create optimization variables for the x and y positions that we will command the \n    # arm to\n    x_locs = prog.NewContinuousVariables(n, \"x\")\n    y_locs = prog.NewContinuousVariables(n, \"y\")\n\n    # Add a squared-distance cost to the points to incentivize the agent to not take a massively \n    # deviating trajectory\n    prog.AddCost(x_locs.dot(x_locs) + y_locs.dot(y_locs))\n\n    def vec_min(x):\n        min_distance = x[0]\n        for i in range(1, n):\n            # min_distance = pydrake.math.min(min_distance, x[i])\n            min_distance = pydrake.symbolic.min(min_distance, x[i])\n\n        return min_distance\n\n\n    def min_distance_constraint(d, x_locs, y_locs):\n        distance_constraint = vec_min([((d[0]-x_locs[i])**2 + (d[1]-y_locs[i])**2) for i in range(n)])\n        # distance_constraint = vec_min([(abs(d[0]-x_locs[i]) + abs(d[1]-y_locs[i])) for i in range(n)])\n        # print(distance_constraint)\n        return distance_constraint\n\n    # for d in dirt_locs:\n    #     prog.AddConstraint(min_distance_constraint(d, x_locs, y_locs) <= 0.1)\n\n\n    # near_dirt_constraint = le(min_distance_constraint(dirt_locs[1], x_locs, y_locs), 0.02)\n    near_dirt_constraint = min_distance_constraint(dirt_locs[1], x_locs, y_locs) <= 0.02\n    prog.AddConstraint(near_dirt_constraint)\n\n    # Constrain the solution such that the first and last positions match start_pos and end_pos\n    # prog.AddConstraint(x_locs[0] == start_pos[0])\n    # prog.AddConstraint(y_locs[0] == start_pos[1])\n    # prog.AddConstraint(x_locs[-1] == end_pos[0])\n    # prog.AddConstraint(y_locs[-1] == end_pos[1])\n    prog.AddConstraint(ge(x_locs, np.zeros(n)))\n    prog.AddConstraint(le(x_locs, np.ones(n) * 2.0))\n    prog.AddConstraint(ge(y_locs, np.ones(n) * -0.7))\n    prog.AddConstraint(le(y_locs, np.ones(n) * 0.7))\n\n    prog.SetInitialGuess(x_locs, np.array([0.8 for i in range(n)]))\n    prog.SetInitialGuess(y_locs, np.array([0.1 for i in range(n)]))\n\n    print(prog)\n    result = Solve(prog)\n\n    # 6. Get the solution\n    if (result.is_success()): \n        sol = result.GetSolution()\n        x_sol = sol[0:n]\n        y_sol = sol[n:]\n        print(f\"X Solution: {x_sol}\")\n        print(f\"Y Solution: {y_sol}\")\n    else:\n        print(\"Failed to Solve :(\")",
   "outputs": [],
   "execution_count": 3
  },
  {
   "cell_type": "code",
   "metadata": {
    "tags": [],
    "cell_id": "00003-772efc52-2979-4a82-a8fc-175e652f19aa",
    "deepnote_to_be_reexecuted": false,
    "source_hash": "3fd43e97",
    "execution_start": 1639025643609,
    "execution_millis": 1,
    "deepnote_output_heights": [
     null,
     283.234375
    ],
    "deepnote_cell_type": "code"
   },
   "source": "dirt_locs = np.array([[0.62, 0.05]])",
   "outputs": [],
   "execution_count": 4
  },
  {
   "cell_type": "code",
   "metadata": {
    "tags": [],
    "deepnote_to_be_reexecuted": false,
    "source_hash": "77fca4a6",
    "execution_start": 1639025643625,
    "execution_millis": 11,
    "deepnote_output_heights": [
     78.703125
    ],
    "cell_id": "00002-2dd52e17-80c2-407d-a4b1-96e297a58537",
    "deepnote_cell_type": "code"
   },
   "source": "# Our controller definition\n\ndef compute_ctrl(p_xyz_now, v_xyz_now, rpy_now, px_des, py_des, fz_des, roll_des, pitch_des, yaw_des, pure_pos_control_mode):\n  \"\"\"Compute control action given current position and velocities, as well as \n  desired x-direction position p_des(t) / desired z-direction force f_des. \n  You may set theta_des yourself, though we recommend regulating it to zero. \n  Input:\n    - p_xyz_now: np.array (dim 3), position of the EE. [px, py, pz] \n    - v_xyz_now: np.array (dim 6), velocity of the EE. [vroll, vpitch, vyaw, vx, vy, vz] \n    - rpy_now: np.array (dim 3), roll, pitch, yaw of the EE\n    - px_des: float, desired x position\n    - py_des: float, desired y position\n    - fz_des: float, desired z force to exert\n    - roll_des: float, desired roll in radians\n    - pitch_des: float, desired pitch in radians\n    - yaw_des: float, desired yaw in radians\n    - pure_pos_control_mode: bool, true if we're in the pure position control setting of our manipulator (i.e, we're not in contact yet), \n      and false otherwise\n  Output:\n    - u: np.array (dim 6), spatial torques to send to the manipulator. [tau_x, tau_y, tau_z, fx, fy, fz]\n    - reached_des: bool, True if the EE currently has reached (epsilon near) the desired x,y position. False otherwise \n  \"\"\"\n  u = np.zeros(6)\n  reached_des = False\n  epsilon = 0.055\n\n  if not pure_pos_control_mode:\n    # D roll\n    kproll = 0.01\n    kdroll = 2.0*np.sqrt(kproll)  # should really be kproll*I on inside\n    # u[0] = kproll * (roll_des - rpy_now[0]) - kdroll*v_xyz_now[0]\n    u[0] = -kdroll*v_xyz_now[0]\n\n    # D pitch\n    kppitch = 0.12\n    kdpitch = 2.0*np.sqrt(kppitch)  # should really be kppitch*I on inside\n    # u[1] = kppitch * (pitch_des - rpy_now[1]) - kdpitch*v_xyz_now[1]\n    u[1] = - kdpitch*v_xyz_now[1]\n\n    # D yaw\n    kpyaw = 0.06\n    kdyaw = 2.0*np.sqrt(kpyaw)  # should really be kpyaw*I on inside\n    # u[2] = kpyaw * (yaw_des - rpy_now[2]) - kdyaw*v_xyz_now[2]\n    u[2] = - kdyaw*v_xyz_now[2]\n\n    # PD X\n    kpx = 6.0\n    kdx = 2.0*np.sqrt(kpx)  # should really be kpx*I on inside\n    u[3] = kpx*(px_des - p_xyz_now[0]) - kdx*v_xyz_now[3]\n\n    # PD Y\n    kpy = 4.0\n    kdy = 2.0*np.sqrt(kpy)  # should really be kpy*I on inside\n    u[4] = kpy*(py_des - p_xyz_now[1]) - kdy*v_xyz_now[4]\n\n    # PD Force Controller Z\n    kpz = 1.25\n    kdz = 0.65\n    u[5] = -kpz * fz_des - kdz*v_xyz_now[5]\n\n    if abs(px_des - p_xyz_now[0]) <= epsilon and abs(py_des - p_xyz_now[1]) <= epsilon:\n      reached_des = True\n\n  return u, reached_des\n\nclass TorqueController(LeafSystem):\n  \"\"\"Wrapper System for Commanding Pure Torques to planar iiwa.\n    @param plant MultibodyPlant of the simulated plant.\n    @param iiwa \n  \"\"\"\n  def __init__(self, plant, iiwa):\n    LeafSystem.__init__(self)\n\n    self._plant = plant \n    self._plant_context = plant.CreateDefaultContext() \n    self._iiwa = iiwa\n    self._G = plant.GetFrameByName(\"iiwa_link_7\", iiwa)\n    self._W = plant.world_frame()\n    self._ctrl_fun = compute_ctrl\n    self._vx = -0.08125\n    self._ik = inverse_kinematics.InverseKinematics(plant)\n    self._curr_position_control_timesteps = 0\n    self._des_position_control_timesteps = 16000*2\n    self._curr_des_xypos_idx = 0\n\n    self.DeclareVectorInputPort(\"iiwa_position_measured\", BasicVector(7))\n    self.DeclareVectorInputPort(\"iiwa_velocity_measured\", BasicVector(7))\n    # self.DeclareVectorInputPort(\"iiwa_torque_external\", BasicVector(7)) # This was added in to do force control on z\n\n    self.DeclareVectorOutputPort(\"iiwa_position_command\", BasicVector(7),\n                                 self.CalcPositionOutput)\n    self.DeclareVectorOutputPort(\"iiwa_torque_cmd\", BasicVector(7),\n                                 self.CalcTorqueOutput)\n    self.DeclareVectorOutputPort(\"iiwa_torque_log\", BasicVector(7),\n                                 self.CalcTorqueOutputLog)\n\n  def CalcPositionOutput(self, context, output):\n    \"\"\"Set q_d = q_now. This ensures the iiwa goes into pure torque mode in sim by setting the \n    position control torques in InverseDynamicsController to zero. \n    NOTE(terry-suh): Do not use this method on hardware or deploy this notebook on hardware. \n    We can only simulate pure torque control mode for iiwa on sim. \n    \"\"\"\n    if self._curr_position_control_timesteps < self._des_position_control_timesteps:\n      # TEMPORARY HACK: Initialize the pose to be on the table\n      q_d = np.array([ 0.08779705,  0.68502306,  0.00395995, -1.60894861, -0.00333842,  0.84763008, 0.09307069]) * (self._curr_position_control_timesteps/self._des_position_control_timesteps)\n      print(q_d)\n      self._curr_position_control_timesteps += 1\n    else:\n      # Set the joint poses to their current values, to simulate torque control\n      q_d = self.get_input_port(0).Eval(context)\n    \n    # print(f\"q_d = {q_d}\")\n    output.SetFromVector(q_d)\n\n  def CalcTorqueOutput(self, context, output):    \n    # Hard-coded position and force profiles. Can be connected from Trajectory class. \n    des_xypos_list = [[0.8, 0.1], [0.55, 0.0]]\n    fz_des = 1.50 #1.25\n    # The desired roll, pitch and yaw are hardcoded to keep the gripper 'upright' w.r.t\n    # the table so wiping can be accomplished correctly.\n    roll_des = 3.13926408 \n    pitch_des = 0.03596836\n    yaw_des = -3.14142808\n\n    # Read inputs \n    q_now = self.get_input_port(0).Eval(context)\n    v_now = self.get_input_port(1).Eval(context)\n\n    self._plant.SetPositions(self._plant_context, self._iiwa, q_now)\n\n    # 1. Retrieve the cartesian location of the gripper (i.e, the last iiwa link)\n    X_now = self._plant.CalcRelativeTransform(self._plant_context, self._W, self._G)\n    rpy_now = RollPitchYaw(X_now.rotation()).vector()\n    p_xyz_now = X_now.translation()\n    print(\"Position:\", p_xyz_now)\n\n    # 2. Compute the Jacobian of the gripper frame w.r.t the robot arm base frame and use \n    # this to derive the cartesian velocity vector of the gripper frame\n    J_G = self._plant.CalcJacobianSpatialVelocity(\n        self._plant_context, JacobianWrtVariable.kQDot, \n        self._G, [0,0,0], self._W, self._W)\n    v_xyz_now = J_G.dot(v_now)\n\n    # 2. Apply ctrl_fun given the position and velocity of the robot to compute the \n    # EE-torque in Cartesian Space\n    if self._curr_position_control_timesteps < self._des_position_control_timesteps: # If we're in pure pos control mode, output all 0's for torque\n      F_xyz, reached_des = self._ctrl_fun(p_xyz_now, v_xyz_now, rpy_now, des_xypos_list[self._curr_des_xypos_idx][0], des_xypos_list[self._curr_des_xypos_idx][1], fz_des, roll_des, pitch_des, yaw_des, True)\n    else: # Else, output the actual torque control command\n      F_xyz, reached_des = self._ctrl_fun(p_xyz_now, v_xyz_now, rpy_now, des_xypos_list[self._curr_des_xypos_idx][0], des_xypos_list[self._curr_des_xypos_idx][1], fz_des, roll_des, pitch_des, yaw_des, False)\n      if reached_des:\n        if self._curr_des_xypos_idx < len(des_xypos_list) - 1:\n          self._curr_des_xypos_idx += 1\n        else:\n          self._curr_des_xypos_idx = 0\n        # print(\"Moving on to next wiping position!\")\n\n    # 3. Convert this torque back to joint space to apply a differential torque command!\n    tau_cmd = J_G.T.dot(F_xyz)\n    #print(\"Force:\", F_xyz)\n    output.SetFromVector(tau_cmd)\n    return tau_cmd\n\n  def CalcTorqueOutputLog(self, context, output):\n    tau = self.CalcTorqueOutput(context, output)\n    # print(\"tau:\", tau)\n    return tau\n\n#####################################",
   "outputs": [],
   "execution_count": 5
  },
  {
   "cell_type": "code",
   "metadata": {
    "tags": [],
    "deepnote_to_be_reexecuted": false,
    "source_hash": "513d638d",
    "execution_start": 1639025643681,
    "execution_millis": 1458455,
    "deepnote_output_heights": [
     null,
     20.734375
    ],
    "cell_id": "00003-de9dbc78-f208-4914-afe1-6bb0eeb6297c",
    "deepnote_cell_type": "code"
   },
   "source": "# Setup wiping bot simulation and run controller\n\ndef setup_wiping_bot(time_step=0.0002):\n    builder = DiagramBuilder()\n\n    # Setting up environment\n    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=time_step)\n    parser = Parser(plant, scene_graph)\n    iiwa = AddIiwa(plant)\n\n    sponge_shape = Box(0.08, 0.065, 0.065)\n    sponge_instance = AddShape(plant, sponge_shape, \"sponge_link\", mass=0.2, mu=0.1, color=[1.0, 1.0, 0.0, 1.0])\n    \n    X_7B = RigidTransform(RotationMatrix(), [0, 0, 0.068])\n    plant.WeldFrames(plant.GetFrameByName(\"iiwa_link_7\", iiwa), plant.GetFrameByName(\"sponge_link\", sponge_instance), X_7B)\n\n\n    # Task Dependent Objects #\n\n   \n    plates_to_wipe = {}\n\n    dirt_shape = pydrake.geometry.Box(0.005, 0.005, 0.005)\n\n    number_dirt_patches = 1\n    number_dirt_per_patch = 10\n\n    dirt = {}\n    dirt[\"num_patches\"] = number_dirt_patches\n    dirt[\"num_per_patch\"] = number_dirt_per_patch\n\n    for i in range(number_dirt_patches):\n        plates_to_wipe[str(i)] = AddShape(plant, pydrake.geometry.Cylinder(0.1, 0.01), \"plate_\" + str(i), mass=1, mu=0.1, color=[1,1,1,1])\n        for j in range(number_dirt_per_patch):\n            dirt[str(i) + \"_\" + str(j)] = AddShape(plant, dirt_shape, \"dirt_\" + str(i) + \"_\" + str(j), mass=0.01, mu=0.1, color=[0,0,0,1])\n\n    print(dirt)\n\n\n    ###\n    table_base = AddShape(plant, pydrake.geometry.Box(0.80, 0.80, 0.15), \"table_to_wipe_base\", mass=15, mu=0.1, color=[0.59,0.29,0.0,1.0])\n    X_0B = RigidTransform(RotationMatrix(), [0.65, 0, 0.070])\n    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"table_to_wipe_base\", table_base), X_0B)\n    plant.Finalize()\n\n    # NOTE: Code taken from notebook setup by Prof. Russ Tedrake \n    # (https://deepnote.com/project/Manipulation-Station-p5S24ob_QPipeARm6RkGqQ/%2Fmanipulation_station.ipynb/#00003-c02ff458-1ca6-4059-9c48-a96ad5962c94)\n    num_iiwa_positions = plant.num_positions(iiwa)\n    iiwa_position = builder.AddSystem(PassThrough(num_iiwa_positions))\n    builder.ExportOutput(iiwa_position.get_output_port(), \"iiwa_position_command\")\n\n    # Export the iiwa \"state\" outputs.\n    demux = builder.AddSystem(Demultiplexer(\n        2 * num_iiwa_positions, num_iiwa_positions))\n    builder.Connect(plant.get_state_output_port(iiwa), demux.get_input_port())\n    builder.ExportOutput(demux.get_output_port(0), \"iiwa_position_measured\")\n    builder.ExportOutput(demux.get_output_port(1), \"iiwa_velocity_estimated\")\n    builder.ExportOutput(plant.get_state_output_port(iiwa), \"iiwa_state_estimated\")\n\n    # Make the plant for the iiwa controller to use.\n    controller_plant = MultibodyPlant(time_step=time_step)\n    controller_iiwa = AddIiwa(controller_plant)\n    controller_plant.Finalize()\n\n    # Add the iiwa controller\n    iiwa_controller = builder.AddSystem(\n        InverseDynamicsController(\n            controller_plant,\n            kp=[100]*num_iiwa_positions,\n            ki=[1]*num_iiwa_positions,\n            kd=[20]*num_iiwa_positions,\n            has_reference_acceleration=False))\n    iiwa_controller.set_name(\"iiwa_controller\")\n    builder.Connect(\n        plant.get_state_output_port(iiwa), iiwa_controller.get_input_port_estimated_state())\n\n    # Add in the feed-forward torque\n    adder = builder.AddSystem(Adder(2, num_iiwa_positions))\n    builder.Connect(iiwa_controller.get_output_port_control(),\n                    adder.get_input_port(0))\n    # Use a PassThrough to make the port optional (it will provide zero values if not connected).\n    torque_passthrough = builder.AddSystem(PassThrough([0]*num_iiwa_positions))\n    builder.Connect(torque_passthrough.get_output_port(),\n                    adder.get_input_port(1))\n    # builder.ExportInput(torque_passthrough.get_input_port(), \n    #                     \"iiwa_feedforward_torque\")\n    builder.Connect(adder.get_output_port(),\n                    plant.get_actuation_input_port(iiwa))\n\n    # Add discrete derivative to command velocities.\n    desired_state_from_position = builder.AddSystem(\n        StateInterpolatorWithDiscreteDerivative(\n            num_iiwa_positions, time_step, suppress_initial_transient=False))\n    desired_state_from_position.set_name(\"desired_state_from_position\")\n    builder.Connect(desired_state_from_position.get_output_port(),      \n                    iiwa_controller.get_input_port_desired_state())\n    builder.Connect(iiwa_position.get_output_port(), \n                    desired_state_from_position.get_input_port())\n\n    # Export \"cheat\" ports.\n    builder.ExportOutput(scene_graph.get_query_output_port(), \"geometry_query\")\n    builder.ExportOutput(plant.get_contact_results_output_port(), \n                         \"contact_results\")\n    builder.ExportOutput(plant.get_state_output_port(), \n                         \"plant_continuous_state\")\n\n    #########################################################################################################################\n\n    # Add the TorqueController for the iiwa\n    logger = builder.AddSystem(VectorLogSink(7))\n    hfp_controller = builder.AddSystem(TorqueController(controller_plant, controller_iiwa))\n    hfp_controller.set_name(\"hfp_controller\")\n    builder.Connect(hfp_controller.get_output_port(0),\n                    iiwa_position.get_input_port()) # This is the iiwa position input port\n    builder.Connect(hfp_controller.get_output_port(1),\n                    torque_passthrough.get_input_port()) # This is the iiwa feefroward torque port\n    builder.Connect(hfp_controller.get_output_port(2),\n                    logger.get_input_port(0))\n    \n    builder.Connect(demux.get_output_port(0), # This is the iiwa position measured port\n                    hfp_controller.get_input_port(0))\n    builder.Connect(demux.get_output_port(1), # This is the iiwa velocity estimated port\n                    hfp_controller.get_input_port(1))\n\n    visualizer = MeshcatVisualizerCpp.AddToBuilder(\n        builder, \n        scene_graph, \n        meshcat,\n        MeshcatVisualizerParams(delete_prefix_initialization_event=False))\n\n    diagram = builder.Build()\n\n    return diagram, plant, plates_to_wipe, dirt\n\n    \nwiping_bot_diagram, plant, plates_to_wipe, dirt = setup_wiping_bot()\ncontext = wiping_bot_diagram.CreateDefaultContext()\nwiping_bot_diagram.Publish(context)\nsimulator = Simulator(wiping_bot_diagram)\n\n# Task Dependent Object Position Settings #\n\nfor i in range(dirt[\"num_patches\"]):\n    x_center = dirt_locs[i][0]\n    y_center = dirt_locs[i][1]\n    plant.SetPositions(plant.GetMyContextFromRoot(simulator.get_mutable_context()), \n        plates_to_wipe[str(i)],\n        np.array([0.707, 0, 0, 0.707, x_center, y_center, 0.17]))\n\nfor i in range(dirt[\"num_patches\"]):\n    x_center = dirt_locs[i][0]\n    y_center = dirt_locs[i][1]\n    for j in range(dirt[\"num_per_patch\"]):\n        xy_pos = np.array([x_center, y_center]) + np.random.rand(2)*0.01 - 0.005\n        dirt_location = np.array([0.707, 0, 0, 0.707, xy_pos[0], xy_pos[1], 0.25])\n        plant.SetPositions(plant.GetMyContextFromRoot(simulator.get_mutable_context()), \n                                dirt[str(i) + \"_\" + str(j)],\n                                dirt_location)\n\n###\nprint(dirt_locs)\n\nsimulator.set_target_realtime_rate(0)\nsimulator.AdvanceTo(50.0)\n",
   "outputs": [
    {
     "name": "stdout",
     "text": "Position: [0.62057798 0.16389449 0.24499404]\nPosition: [0.62057798 0.16389449 0.24499404]\nPosition: [0.62057745 0.16389445 0.24499404]\nPosition: [0.62057745 0.16389445 0.24499404]\nPosition: [0.62057692 0.16389441 0.24499404]\nPosition: [0.62057692 0.16389441 0.24499404]\nPosition: [0.62057639 0.16389437 0.24499404]\nPosition: [0.62057639 0.16389437 0.24499404]\nPosition: [0.62057586 0.16389433 0.24499404]\nPosition: [0.62057586 0.16389433 0.24499404]\nPosition: [0.62057533 0.16389429 0.24499404]\nPosition: [0.62057533 0.16389429 0.24499404]\nPosition: [0.6205748  0.16389425 0.24499404]\nPosition: [0.6205748  0.16389425 0.24499404]\nPosition: [0.62057427 0.16389422 0.24499404]\nPosition: [0.62057427 0.16389422 0.24499404]\nPosition: [0.62057375 0.16389418 0.24499404]\nPosition: [0.62057375 0.16389418 0.24499404]\nPosition: [0.62057322 0.16389414 0.24499404]\nPosition: [0.62057322 0.16389414 0.24499404]\nPosition: [0.62057269 0.1638941  0.24499404]\nPosition: [0.62057269 0.1638941  0.24499404]\nPosition: [0.62057217 0.16389406 0.24499404]\nPosition: [0.62057217 0.16389406 0.24499404]\nPosition: [0.62057164 0.16389402 0.24499404]\nPosition: [0.62057164 0.16389402 0.24499404]\nPosition: [0.62057112 0.16389398 0.24499404]\nPosition: [0.62057112 0.16389398 0.24499404]\nPosition: [0.62057059 0.16389395 0.24499404]\nPosition: [0.62057059 0.16389395 0.24499404]\nPosition: [0.62057007 0.16389391 0.24499404]\nPosition: [0.62057007 0.16389391 0.24499404]\nPosition: [0.62056955 0.16389387 0.24499404]\nPosition: [0.62056955 0.16389387 0.24499404]\nPosition: [0.62056903 0.16389383 0.24499404]\nPosition: [0.62056903 0.16389383 0.24499404]\nPosition: [0.6205685  0.16389379 0.24499404]\nPosition: [0.6205685  0.16389379 0.24499404]\nPosition: [0.62056798 0.16389375 0.24499404]\nPosition: [0.62056798 0.16389375 0.24499404]\nPosition: [0.62056746 0.16389371 0.24499404]\nPosition: [0.62056746 0.16389371 0.24499404]\nPosition: [0.62056694 0.16389367 0.24499404]\nPosition: [0.62056694 0.16389367 0.24499404]\nPosition: [0.62056643 0.16389364 0.24499404]\nPosition: [0.62056643 0.16389364 0.24499404]\nPosition: [0.62056591 0.1638936  0.24499404]\nPosition: [0.62056591 0.1638936  0.24499404]\nPosition: [0.62056539 0.16389356 0.24499404]\nPosition: [0.62056539 0.16389356 0.24499404]\nPosition: [0.62056487 0.16389352 0.24499404]\nPosition: [0.62056487 0.16389352 0.24499404]\nPosition: [0.62056436 0.16389348 0.24499404]\nPosition: [0.62056436 0.16389348 0.24499404]\nPosition: [0.62056384 0.16389344 0.24499403]\nPosition: [0.62056384 0.16389344 0.24499403]\nPosition: [0.62056332 0.1638934  0.24499404]\nPosition: [0.62056332 0.1638934  0.24499404]\nPosition: [0.62056281 0.16389336 0.24499404]\nPosition: [0.62056281 0.16389336 0.24499404]\nPosition: [0.6205623  0.16389333 0.24499404]\nPosition: [0.6205623  0.16389333 0.24499404]\nPosition: [0.62056178 0.16389329 0.24499403]\nPosition: [0.62056178 0.16389329 0.24499403]\nPosition: [0.62056127 0.16389325 0.24499404]\nPosition: [0.62056127 0.16389325 0.24499404]\nPosition: [0.62056076 0.16389321 0.24499404]\nPosition: [0.62056076 0.16389321 0.24499404]\nPosition: [0.62056025 0.16389317 0.24499404]\nPosition: [0.62056025 0.16389317 0.24499404]\nPosition: [0.62055974 0.16389313 0.24499403]\nPosition: [0.62055974 0.16389313 0.24499403]\nPosition: [0.62055923 0.16389309 0.24499404]\nPosition: [0.62055923 0.16389309 0.24499404]\nPosition: [0.62055872 0.16389305 0.24499404]\nPosition: [0.62055872 0.16389305 0.24499404]\nPosition: [0.62055821 0.16389301 0.24499404]\nPosition: [0.62055821 0.16389301 0.24499404]\nPosition: [0.6205577  0.16389297 0.24499403]\nPosition: [0.6205577  0.16389297 0.24499403]\nPosition: [0.62055719 0.16389294 0.24499404]\nPosition: [0.62055719 0.16389294 0.24499404]\nPosition: [0.62055668 0.1638929  0.24499404]\nPosition: [0.62055668 0.1638929  0.24499404]\nPosition: [0.62055618 0.16389286 0.24499403]\nPosition: [0.62055618 0.16389286 0.24499403]\nPosition: [0.62055567 0.16389282 0.24499403]\nPosition: [0.62055567 0.16389282 0.24499403]\nPosition: [0.62055517 0.16389278 0.24499404]\nPosition: [0.62055517 0.16389278 0.24499404]\nPosition: [0.62055466 0.16389274 0.24499404]\nPosition: [0.62055466 0.16389274 0.24499404]\nPosition: [0.62055416 0.1638927  0.24499403]\nPosition: [0.62055416 0.1638927  0.24499403]\nPosition: [0.62055365 0.16389266 0.24499403]\nPosition: [0.62055365 0.16389266 0.24499403]\nPosition: [0.62055315 0.16389262 0.24499403]\nPosition: [0.62055315 0.16389262 0.24499403]\nPosition: [0.62055265 0.16389258 0.24499404]\nPosition: [0.62055265 0.16389258 0.24499404]\nPosition: [0.62055215 0.16389254 0.24499403]\nPosition: [0.62055215 0.16389254 0.24499403]\nPosition: [0.62055165 0.16389251 0.24499403]\nPosition: [0.62055165 0.16389251 0.24499403]\nPosition: [0.62055114 0.16389247 0.24499403]\nPosition: [0.62055114 0.16389247 0.24499403]\nPosition: [0.62055065 0.16389243 0.24499404]\nPosition: [0.62055065 0.16389243 0.24499404]\nPosition: [0.62055015 0.16389239 0.24499403]\nPosition: [0.62055015 0.16389239 0.24499403]\nPosition: [0.62054965 0.16389235 0.24499403]\nPosition: [0.62054965 0.16389235 0.24499403]\nPosition: [0.62054915 0.16389231 0.24499403]\nPosition: [0.62054915 0.16389231 0.24499403]\nPosition: [0.62054865 0.16389227 0.24499403]\nPosition: [0.62054865 0.16389227 0.24499403]\nPosition: [0.62054815 0.16389223 0.24499403]\nPosition: [0.62054815 0.16389223 0.24499403]\nPosition: [0.62054766 0.16389219 0.24499403]\nPosition: [0.62054766 0.16389219 0.24499403]\nPosition: [0.62054716 0.16389215 0.24499403]\nPosition: [0.62054716 0.16389215 0.24499403]\nPosition: [0.62054667 0.16389211 0.24499403]\nPosition: [0.62054667 0.16389211 0.24499403]\nPosition: [0.62054617 0.16389207 0.24499403]\nPosition: [0.62054617 0.16389207 0.24499403]\nPosition: [0.62054568 0.16389203 0.24499403]\nPosition: [0.62054568 0.16389203 0.24499403]\nPosition: [0.62054519 0.163892   0.24499403]\nPosition: [0.62054519 0.163892   0.24499403]\nPosition: [0.62054469 0.16389196 0.24499403]\nPosition: [0.62054469 0.16389196 0.24499403]\nPosition: [0.6205442  0.16389192 0.24499403]\nPosition: [0.6205442  0.16389192 0.24499403]\nPosition: [0.62054371 0.16389188 0.24499403]\nPosition: [0.62054371 0.16389188 0.24499403]\nPosition: [0.62054322 0.16389184 0.24499403]\nPosition: [0.62054322 0.16389184 0.24499403]\nPosition: [0.62054273 0.1638918  0.24499403]\nPosition: [0.62054273 0.1638918  0.24499403]\nPosition: [0.62054224 0.16389176 0.24499403]\nPosition: [0.62054224 0.16389176 0.24499403]\nPosition: [0.62054175 0.16389172 0.24499403]\nPosition: [0.62054175 0.16389172 0.24499403]\nPosition: [0.62054126 0.16389168 0.24499403]\nPosition: [0.62054126 0.16389168 0.24499403]\nPosition: [0.62054078 0.16389164 0.24499403]\nPosition: [0.62054078 0.16389164 0.24499403]\nPosition: [0.62054029 0.1638916  0.24499403]\nPosition: [0.62054029 0.1638916  0.24499403]\nPosition: [0.6205398  0.16389156 0.24499403]\nPosition: [0.6205398  0.16389156 0.24499403]\nPosition: [0.62053932 0.16389152 0.24499403]\nPosition: [0.62053932 0.16389152 0.24499403]\nPosition: [0.62053883 0.16389148 0.24499403]\nPosition: [0.62053883 0.16389148 0.24499403]\nPosition: [0.62053835 0.16389144 0.24499403]\nPosition: [0.62053835 0.16389144 0.24499403]\nPosition: [0.62053786 0.1638914  0.24499403]\nPosition: [0.62053786 0.1638914  0.24499403]\nPosition: [0.62053738 0.16389136 0.24499403]\nPosition: [0.62053738 0.16389136 0.24499403]\nPosition: [0.6205369  0.16389132 0.24499403]\nPosition: [0.6205369  0.16389132 0.24499403]\nPosition: [0.62053641 0.16389128 0.24499403]\nPosition: [0.62053641 0.16389128 0.24499403]\nPosition: [0.62053593 0.16389125 0.24499403]\nPosition: [0.62053593 0.16389125 0.24499403]\nPosition: [0.62053545 0.16389121 0.24499403]\nPosition: [0.62053545 0.16389121 0.24499403]\nPosition: [0.62053497 0.16389117 0.24499403]\nPosition: [0.62053497 0.16389117 0.24499403]\nPosition: [0.62053449 0.16389113 0.24499403]\nPosition: [0.62053449 0.16389113 0.24499403]\nPosition: [0.62053401 0.16389109 0.24499403]\nPosition: [0.62053401 0.16389109 0.24499403]\nPosition: [0.62053353 0.16389105 0.24499403]\nPosition: [0.62053353 0.16389105 0.24499403]\nPosition: [0.62053306 0.16389101 0.24499403]\nPosition: [0.62053306 0.16389101 0.24499403]\nPosition: [0.62053258 0.16389097 0.24499403]\nPosition: [0.62053258 0.16389097 0.24499403]\nPosition: [0.6205321  0.16389093 0.24499403]\nPosition: [0.6205321  0.16389093 0.24499403]\nPosition: [0.62053163 0.16389089 0.24499403]\nPosition: [0.62053163 0.16389089 0.24499403]\nPosition: [0.62053115 0.16389085 0.24499403]\nPosition: [0.62053115 0.16389085 0.24499403]\nPosition: [0.62053068 0.16389081 0.24499403]\nPosition: [0.62053068 0.16389081 0.24499403]\nPosition: [0.6205302  0.16389077 0.24499403]\nPosition: [0.6205302  0.16389077 0.24499403]\nPosition: [0.62052973 0.16389073 0.24499403]\nPosition: [0.62052973 0.16389073 0.24499403]\nPosition: [0.62052925 0.16389069 0.24499403]\nPosition: [0.62052925 0.16389069 0.24499403]\nPosition: [0.62052878 0.16389065 0.24499403]\nPosition: [0.62052878 0.16389065 0.24499403]\nPosition: [0.62052831 0.16389061 0.24499403]\nPosition: [0.62052831 0.16389061 0.24499403]\nPosition: [0.62052784 0.16389057 0.24499403]\nPosition: [0.62052784 0.16389057 0.24499403]\nPosition: [0.62052737 0.16389053 0.24499403]\nPosition: [0.62052737 0.16389053 0.24499403]\nPosition: [0.6205269  0.16389049 0.24499403]\nPosition: [0.6205269  0.16389049 0.24499403]\nPosition: [0.62052643 0.16389045 0.24499403]\nPosition: [0.62052643 0.16389045 0.24499403]\nPosition: [0.62052596 0.16389041 0.24499403]\nPosition: [0.62052596 0.16389041 0.24499403]\nPosition: [0.62052549 0.16389037 0.24499403]\nPosition: [0.62052549 0.16389037 0.24499403]\nPosition: [0.62052502 0.16389033 0.24499403]\nPosition: [0.62052502 0.16389033 0.24499403]\nPosition: [0.62052456 0.16389029 0.24499403]\nPosition: [0.62052456 0.16389029 0.24499403]\nPosition: [0.62052409 0.16389025 0.24499403]\nPosition: [0.62052409 0.16389025 0.24499403]\nPosition: [0.62052362 0.16389021 0.24499403]\nPosition: [0.62052362 0.16389021 0.24499403]\nPosition: [0.62052316 0.16389017 0.24499403]\nPosition: [0.62052316 0.16389017 0.24499403]\nPosition: [0.62052269 0.16389013 0.24499403]\nPosition: [0.62052269 0.16389013 0.24499403]\nPosition: [0.62052223 0.16389009 0.24499403]\nPosition: [0.62052223 0.16389009 0.24499403]\nPosition: [0.62052177 0.16389005 0.24499403]\nPosition: [0.62052177 0.16389005 0.24499403]\nPosition: [0.6205213  0.16389001 0.24499403]\nPosition: [0.6205213  0.16389001 0.24499403]\nPosition: [0.62052084 0.16388997 0.24499403]\nPosition: [0.62052084 0.16388997 0.24499403]\nPosition: [0.62052038 0.16388993 0.24499403]\nPosition: [0.62052038 0.16388993 0.24499403]\nPosition: [0.62051992 0.16388989 0.24499403]\nPosition: [0.62051992 0.16388989 0.24499403]\nPosition: [0.62051946 0.16388985 0.24499403]\nPosition: [0.62051946 0.16388985 0.24499403]\nPosition: [0.620519   0.16388981 0.24499403]\nPosition: [0.620519   0.16388981 0.24499403]\nPosition: [0.62051854 0.16388977 0.24499403]\nPosition: [0.62051854 0.16388977 0.24499403]\nPosition: [0.62051808 0.16388973 0.24499403]\nPosition: [0.62051808 0.16388973 0.24499403]\nPosition: [0.62051762 0.16388969 0.24499403]\nPosition: [0.62051762 0.16388969 0.24499403]\nPosition: [0.62051717 0.16388965 0.24499403]\nPosition: [0.62051717 0.16388965 0.24499403]\nPosition: [0.62051671 0.16388961 0.24499403]\nPosition: [0.62051671 0.16388961 0.24499403]\nPosition: [0.62051625 0.16388957 0.24499403]\nPosition: [0.62051625 0.16388957 0.24499403]\nPosition: [0.6205158  0.16388953 0.24499403]\nPosition: [0.6205158  0.16388953 0.24499403]\nPosition: [0.62051534 0.16388949 0.24499403]\nPosition: [0.62051534 0.16388949 0.24499403]\nPosition: [0.62051489 0.16388945 0.24499403]\nPosition: [0.62051489 0.16388945 0.24499403]\nPosition: [0.62051443 0.16388941 0.24499403]\nPosition: [0.62051443 0.16388941 0.24499403]\nPosition: [0.62051398 0.16388937 0.24499403]\nPosition: [0.62051398 0.16388937 0.24499403]\nPosition: [0.62051353 0.16388933 0.24499403]\nPosition: [0.62051353 0.16388933 0.24499403]\nPosition: [0.62051308 0.16388929 0.24499403]\nPosition: [0.62051308 0.16388929 0.24499403]\nPosition: [0.62051263 0.16388925 0.24499403]\nPosition: [0.62051263 0.16388925 0.24499403]\nPosition: [0.62051217 0.16388921 0.24499403]\nPosition: [0.62051217 0.16388921 0.24499403]\nPosition: [0.62051172 0.16388917 0.24499403]\nPosition: [0.62051172 0.16388917 0.24499403]\nPosition: [0.62051127 0.16388913 0.24499403]\nPosition: [0.62051127 0.16388913 0.24499403]\nPosition: [0.62051083 0.16388909 0.24499403]\nPosition: [0.62051083 0.16388909 0.24499403]\nPosition: [0.62051038 0.16388905 0.24499403]\nPosition: [0.62051038 0.16388905 0.24499403]\nPosition: [0.62050993 0.16388901 0.24499403]\nPosition: [0.62050993 0.16388901 0.24499403]\nPosition: [0.62050948 0.16388897 0.24499403]\nPosition: [0.62050948 0.16388897 0.24499403]\nPosition: [0.62050904 0.16388893 0.24499403]\nPosition: [0.62050904 0.16388893 0.24499403]\nPosition: [0.62050859 0.16388889 0.24499403]\nPosition: [0.62050859 0.16388889 0.24499403]\nPosition: [0.62050814 0.16388885 0.24499403]\nPosition: [0.62050814 0.16388885 0.24499403]\nPosition: [0.6205077  0.16388881 0.24499403]\nPosition: [0.6205077  0.16388881 0.24499403]\nPosition: [0.62050726 0.16388877 0.24499403]\nPosition: [0.62050726 0.16388877 0.24499403]\nPosition: [0.62050681 0.16388873 0.24499403]\nPosition: [0.62050681 0.16388873 0.24499403]\nPosition: [0.62050637 0.16388869 0.24499403]\nPosition: [0.62050637 0.16388869 0.24499403]\nPosition: [0.62050637 0.16388869 0.24499403]\nPosition: [0.62050593 0.16388865 0.24499403]\nPosition: [0.62050593 0.16388865 0.24499403]\nPosition: [0.62050549 0.16388861 0.24499403]\nPosition: [0.62050549 0.16388861 0.24499403]\nPosition: [0.62050504 0.16388856 0.24499403]\nPosition: [0.62050504 0.16388856 0.24499403]\nPosition: [0.6205046  0.16388852 0.24499403]\nPosition: [0.6205046  0.16388852 0.24499403]\nPosition: [0.62050416 0.16388848 0.24499403]\nPosition: [0.62050416 0.16388848 0.24499403]\nPosition: [0.62050372 0.16388844 0.24499403]\nPosition: [0.62050372 0.16388844 0.24499403]\nPosition: [0.62050329 0.1638884  0.24499403]\nPosition: [0.62050329 0.1638884  0.24499403]\nPosition: [0.62050285 0.16388836 0.24499403]\nPosition: [0.62050285 0.16388836 0.24499403]\nPosition: [0.62050241 0.16388832 0.24499403]\nPosition: [0.62050241 0.16388832 0.24499403]\nPosition: [0.62050197 0.16388828 0.24499403]\nPosition: [0.62050197 0.16388828 0.24499403]\nPosition: [0.62050154 0.16388824 0.24499403]\nPosition: [0.62050154 0.16388824 0.24499403]\nPosition: [0.6205011  0.1638882  0.24499403]\nPosition: [0.6205011  0.1638882  0.24499403]\nPosition: [0.62050067 0.16388816 0.24499403]\nPosition: [0.62050067 0.16388816 0.24499403]\nPosition: [0.62050023 0.16388812 0.24499403]\nPosition: [0.62050023 0.16388812 0.24499403]\nPosition: [0.6204998  0.16388808 0.24499403]\nPosition: [0.6204998  0.16388808 0.24499403]\nPosition: [0.62049936 0.16388804 0.24499403]\nPosition: [0.62049936 0.16388804 0.24499403]\nPosition: [0.62049893 0.163888   0.24499403]\nPosition: [0.62049893 0.163888   0.24499403]\nPosition: [0.6204985  0.16388796 0.24499403]\nPosition: [0.6204985  0.16388796 0.24499403]\nPosition: [0.62049807 0.16388792 0.24499403]\nPosition: [0.62049807 0.16388792 0.24499403]\nPosition: [0.62049764 0.16388788 0.24499403]\nPosition: [0.62049764 0.16388788 0.24499403]\nPosition: [0.6204972  0.16388784 0.24499403]\nPosition: [0.6204972  0.16388784 0.24499403]\nPosition: [0.62049678 0.16388779 0.24499403]\nPosition: [0.62049678 0.16388779 0.24499403]\nPosition: [0.62049635 0.16388775 0.24499403]\nPosition: [0.62049635 0.16388775 0.24499403]\nPosition: [0.62049592 0.16388771 0.24499403]\nPosition: [0.62049592 0.16388771 0.24499403]\nPosition: [0.62049549 0.16388767 0.24499403]\nPosition: [0.62049549 0.16388767 0.24499403]\nPosition: [0.62049506 0.16388763 0.24499403]\nPosition: [0.62049506 0.16388763 0.24499403]\nPosition: [0.62049463 0.16388759 0.24499403]\nPosition: [0.62049463 0.16388759 0.24499403]\nPosition: [0.62049421 0.16388755 0.24499403]\nPosition: [0.62049421 0.16388755 0.24499403]\nPosition: [0.62049378 0.16388751 0.24499403]\nPosition: [0.62049378 0.16388751 0.24499403]\nPosition: [0.62049336 0.16388747 0.24499403]\nPosition: [0.62049336 0.16388747 0.24499403]\nPosition: [0.62049293 0.16388743 0.24499403]\nPosition: [0.62049293 0.16388743 0.24499403]\nPosition: [0.62049251 0.16388739 0.24499403]\nPosition: [0.62049251 0.16388739 0.24499403]\nPosition: [0.62049209 0.16388735 0.24499403]\nPosition: [0.62049209 0.16388735 0.24499403]\nPosition: [0.62049166 0.16388731 0.24499403]\nPosition: [0.62049166 0.16388731 0.24499403]\nPosition: [0.62049124 0.16388727 0.24499403]\nPosition: [0.62049124 0.16388727 0.24499403]\nPosition: [0.62049082 0.16388723 0.24499403]\nPosition: [0.62049082 0.16388723 0.24499403]\nPosition: [0.6204904  0.16388718 0.24499403]\nPosition: [0.6204904  0.16388718 0.24499403]\nPosition: [0.62048998 0.16388714 0.24499403]\nPosition: [0.62048998 0.16388714 0.24499403]\nPosition: [0.62048956 0.1638871  0.24499403]\nPosition: [0.62048956 0.1638871  0.24499403]\nPosition: [0.62048914 0.16388706 0.24499403]\nPosition: [0.62048914 0.16388706 0.24499403]\nPosition: [0.62048872 0.16388702 0.24499403]\nPosition: [0.62048872 0.16388702 0.24499403]\nPosition: [0.6204883  0.16388698 0.24499403]\nPosition: [0.6204883  0.16388698 0.24499403]\nPosition: [0.62048789 0.16388694 0.24499403]\nPosition: [0.62048789 0.16388694 0.24499403]\nPosition: [0.62048747 0.1638869  0.24499403]\nPosition: [0.62048747 0.1638869  0.24499403]\nPosition: [0.62048705 0.16388686 0.24499403]\nPosition: [0.62048705 0.16388686 0.24499403]\nPosition: [0.62048664 0.16388682 0.24499403]\nPosition: [0.62048664 0.16388682 0.24499403]\nPosition: [0.62048622 0.16388678 0.24499403]\nPosition: [0.62048622 0.16388678 0.24499403]\nPosition: [0.62048581 0.16388674 0.24499403]\nPosition: [0.62048581 0.16388674 0.24499403]\nPosition: [0.62048539 0.16388669 0.24499403]\nPosition: [0.62048539 0.16388669 0.24499403]\nPosition: [0.62048498 0.16388665 0.24499403]\nPosition: [0.62048498 0.16388665 0.24499403]\nPosition: [0.62048457 0.16388661 0.24499403]\nPosition: [0.62048457 0.16388661 0.24499403]\nPosition: [0.62048416 0.16388657 0.24499403]\nPosition: [0.62048416 0.16388657 0.24499403]\nPosition: [0.62048375 0.16388653 0.24499403]\nPosition: [0.62048375 0.16388653 0.24499403]\nPosition: [0.62048333 0.16388649 0.24499403]\nPosition: [0.62048333 0.16388649 0.24499403]\nPosition: [0.62048292 0.16388645 0.24499403]\nPosition: [0.62048292 0.16388645 0.24499403]\nPosition: [0.62048252 0.16388641 0.24499403]\nPosition: [0.62048252 0.16388641 0.24499403]\nPosition: [0.62048211 0.16388637 0.24499403]\nPosition: [0.62048211 0.16388637 0.24499403]\nPosition: [0.6204817  0.16388633 0.24499403]\nPosition: [0.6204817  0.16388633 0.24499403]\nPosition: [0.62048129 0.16388628 0.24499403]\nPosition: [0.62048129 0.16388628 0.24499403]\nPosition: [0.62048088 0.16388624 0.24499403]\nPosition: [0.62048088 0.16388624 0.24499403]\nPosition: [0.62048048 0.1638862  0.24499403]\nPosition: [0.62048048 0.1638862  0.24499403]\nPosition: [0.62048007 0.16388616 0.24499403]\nPosition: [0.62048007 0.16388616 0.24499403]\nPosition: [0.62047966 0.16388612 0.24499403]\nPosition: [0.62047966 0.16388612 0.24499403]\nPosition: [0.62047926 0.16388608 0.24499403]\nPosition: [0.62047926 0.16388608 0.24499403]\nPosition: [0.62047885 0.16388604 0.24499403]\nPosition: [0.62047885 0.16388604 0.24499403]\nPosition: [0.62047845 0.163886   0.24499403]\nPosition: [0.62047845 0.163886   0.24499403]\nPosition: [0.62047805 0.16388596 0.24499403]\nPosition: [0.62047805 0.16388596 0.24499403]\nPosition: [0.62047765 0.16388591 0.24499403]\nPosition: [0.62047765 0.16388591 0.24499403]\nPosition: [0.62047724 0.16388587 0.24499403]\nPosition: [0.62047724 0.16388587 0.24499403]\nPosition: [0.62047684 0.16388583 0.24499403]\nPosition: [0.62047684 0.16388583 0.24499403]\nPosition: [0.62047644 0.16388579 0.24499403]\nPosition: [0.62047644 0.16388579 0.24499403]\nPosition: [0.62047604 0.16388575 0.24499403]\nPosition: [0.62047604 0.16388575 0.24499403]\nPosition: [0.62047564 0.16388571 0.24499403]\nPosition: [0.62047564 0.16388571 0.24499403]\nPosition: [0.62047524 0.16388567 0.24499403]\nPosition: [0.62047524 0.16388567 0.24499403]\nPosition: [0.62047484 0.16388563 0.24499403]\nPosition: [0.62047484 0.16388563 0.24499403]\nPosition: [0.62047445 0.16388559 0.24499403]\nPosition: [0.62047445 0.16388559 0.24499403]\nPosition: [0.62047405 0.16388554 0.24499403]\nPosition: [0.62047405 0.16388554 0.24499403]\nPosition: [0.62047365 0.1638855  0.24499403]\nPosition: [0.62047365 0.1638855  0.24499403]\nPosition: [0.62047365 0.1638855  0.24499403]\nPosition: [0.62047326 0.16388546 0.24499403]\nPosition: [0.62047326 0.16388546 0.24499403]\nPosition: [0.62047286 0.16388542 0.24499403]\nPosition: [0.62047286 0.16388542 0.24499403]\nPosition: [0.62047247 0.16388538 0.24499403]\nPosition: [0.62047247 0.16388538 0.24499403]\nPosition: [0.62047207 0.16388534 0.24499403]\nPosition: [0.62047207 0.16388534 0.24499403]\nPosition: [0.62047168 0.1638853  0.24499403]\nPosition: [0.62047168 0.1638853  0.24499403]\nPosition: [0.62047128 0.16388526 0.24499403]\nPosition: [0.62047128 0.16388526 0.24499403]\nPosition: [0.62047089 0.16388521 0.24499403]\nPosition: [0.62047089 0.16388521 0.24499403]\nPosition: [0.6204705  0.16388517 0.24499403]\nPosition: [0.6204705  0.16388517 0.24499403]\nPosition: [0.62047011 0.16388513 0.24499403]\nPosition: [0.62047011 0.16388513 0.24499403]\nPosition: [0.62046972 0.16388509 0.24499403]\nPosition: [0.62046972 0.16388509 0.24499403]\nPosition: [0.62046933 0.16388505 0.24499403]\nPosition: [0.62046933 0.16388505 0.24499403]\nPosition: [0.62046894 0.16388501 0.24499403]\nPosition: [0.62046894 0.16388501 0.24499403]\nPosition: [0.62046855 0.16388497 0.24499403]\nPosition: [0.62046855 0.16388497 0.24499403]\nPosition: [0.62046816 0.16388492 0.24499403]\nPosition: [0.62046816 0.16388492 0.24499403]\nPosition: [0.62046777 0.16388488 0.24499403]\nPosition: [0.62046777 0.16388488 0.24499403]\nPosition: [0.62046739 0.16388484 0.24499403]\nPosition: [0.62046739 0.16388484 0.24499403]\nPosition: [0.620467   0.1638848  0.24499403]\nPosition: [0.620467   0.1638848  0.24499403]\nPosition: [0.62046661 0.16388476 0.24499403]\nPosition: [0.62046661 0.16388476 0.24499403]\nPosition: [0.62046623 0.16388472 0.24499403]\nPosition: [0.62046623 0.16388472 0.24499403]\nPosition: [0.62046584 0.16388468 0.24499403]\nPosition: [0.62046584 0.16388468 0.24499403]\nPosition: [0.62046546 0.16388463 0.24499403]\nPosition: [0.62046546 0.16388463 0.24499403]\nPosition: [0.62046507 0.16388459 0.24499403]\nPosition: [0.62046507 0.16388459 0.24499403]\nPosition: [0.62046469 0.16388455 0.24499403]\nPosition: [0.62046469 0.16388455 0.24499403]\nPosition: [0.62046431 0.16388451 0.24499403]\nPosition: [0.62046431 0.16388451 0.24499403]\nPosition: [0.62046393 0.16388447 0.24499403]\nPosition: [0.62046393 0.16388447 0.24499403]\nPosition: [0.62046354 0.16388443 0.24499403]\nPosition: [0.62046354 0.16388443 0.24499403]\nPosition: [0.62046316 0.16388439 0.24499403]\nPosition: [0.62046316 0.16388439 0.24499403]\nPosition: [0.62046278 0.16388434 0.24499403]\nPosition: [0.62046278 0.16388434 0.24499403]\nPosition: [0.6204624  0.1638843  0.24499403]\nPosition: [0.6204624  0.1638843  0.24499403]\nPosition: [0.62046202 0.16388426 0.24499403]\nPosition: [0.62046202 0.16388426 0.24499403]\nPosition: [0.62046165 0.16388422 0.24499403]\nPosition: [0.62046165 0.16388422 0.24499403]\nPosition: [0.62046127 0.16388418 0.24499403]\nPosition: [0.62046127 0.16388418 0.24499403]\nPosition: [0.62046089 0.16388414 0.24499403]\nPosition: [0.62046089 0.16388414 0.24499403]\nPosition: [0.62046051 0.16388409 0.24499403]\nPosition: [0.62046051 0.16388409 0.24499403]\nPosition: [0.62046014 0.16388405 0.24499403]\nPosition: [0.62046014 0.16388405 0.24499403]\nPosition: [0.62045976 0.16388401 0.24499403]\nPosition: [0.62045976 0.16388401 0.24499403]\nPosition: [0.62045939 0.16388397 0.24499403]\nPosition: [0.62045939 0.16388397 0.24499403]\nPosition: [0.62045901 0.16388393 0.24499403]\nPosition: [0.62045901 0.16388393 0.24499403]\nPosition: [0.62045864 0.16388389 0.24499403]\nPosition: [0.62045864 0.16388389 0.24499403]\nPosition: [0.62045827 0.16388384 0.24499403]\nPosition: [0.62045827 0.16388384 0.24499403]\nPosition: [0.62045789 0.1638838  0.24499403]\nPosition: [0.62045789 0.1638838  0.24499403]\nPosition: [0.62045752 0.16388376 0.24499403]\nPosition: [0.62045752 0.16388376 0.24499403]\nPosition: [0.62045715 0.16388372 0.24499403]\nPosition: [0.62045715 0.16388372 0.24499403]\nPosition: [0.62045678 0.16388368 0.24499403]\nPosition: [0.62045678 0.16388368 0.24499403]\nPosition: [0.62045641 0.16388364 0.24499403]\nPosition: [0.62045641 0.16388364 0.24499403]\nPosition: [0.62045604 0.1638836  0.24499403]\nPosition: [0.62045604 0.1638836  0.24499403]\nPosition: [0.62045567 0.16388355 0.24499403]\nPosition: [0.62045567 0.16388355 0.24499403]\nPosition: [0.6204553  0.16388351 0.24499403]\nPosition: [0.6204553  0.16388351 0.24499403]\nPosition: [0.62045493 0.16388347 0.24499403]\nPosition: [0.62045493 0.16388347 0.24499403]\nPosition: [0.62045456 0.16388343 0.24499403]\nPosition: [0.62045456 0.16388343 0.24499403]\nPosition: [0.6204542  0.16388339 0.24499403]\nPosition: [0.6204542  0.16388339 0.24499403]\nPosition: [0.62045383 0.16388334 0.24499403]\nPosition: [0.62045383 0.16388334 0.24499403]\nPosition: [0.62045347 0.1638833  0.24499403]\nPosition: [0.62045347 0.1638833  0.24499403]\nPosition: [0.6204531  0.16388326 0.24499403]\nPosition: [0.6204531  0.16388326 0.24499403]\nPosition: [0.62045274 0.16388322 0.24499403]\nPosition: [0.62045274 0.16388322 0.24499403]\nPosition: [0.62045237 0.16388318 0.24499403]\nPosition: [0.62045237 0.16388318 0.24499403]\nPosition: [0.62045201 0.16388314 0.24499403]\nPosition: [0.62045201 0.16388314 0.24499403]\nPosition: [0.62045165 0.16388309 0.24499403]\nPosition: [0.62045165 0.16388309 0.24499403]\nPosition: [0.62045128 0.16388305 0.24499403]\nPosition: [0.62045128 0.16388305 0.24499403]\nPosition: [0.62045092 0.16388301 0.24499403]\nPosition: [0.62045092 0.16388301 0.24499403]\nPosition: [0.62045056 0.16388297 0.24499403]\nPosition: [0.62045056 0.16388297 0.24499403]\nPosition: [0.6204502  0.16388293 0.24499403]\nPosition: [0.6204502  0.16388293 0.24499403]\nPosition: [0.62044984 0.16388288 0.24499403]\nPosition: [0.62044984 0.16388288 0.24499403]\nPosition: [0.62044948 0.16388284 0.24499403]\nPosition: [0.62044948 0.16388284 0.24499403]\nPosition: [0.62044912 0.1638828  0.24499403]\nPosition: [0.62044912 0.1638828  0.24499403]\nPosition: [0.62044876 0.16388276 0.24499403]\nPosition: [0.62044876 0.16388276 0.24499403]\nPosition: [0.62044841 0.16388272 0.24499403]\nPosition: [0.62044841 0.16388272 0.24499403]\nPosition: [0.62044805 0.16388268 0.24499403]\nPosition: [0.62044805 0.16388268 0.24499403]\nPosition: [0.62044769 0.16388263 0.24499403]\nPosition: [0.62044769 0.16388263 0.24499403]\nPosition: [0.62044734 0.16388259 0.24499403]\nPosition: [0.62044734 0.16388259 0.24499403]\nPosition: [0.62044698 0.16388255 0.24499403]\nPosition: [0.62044698 0.16388255 0.24499403]\nPosition: [0.62044663 0.16388251 0.24499403]\nPosition: [0.62044663 0.16388251 0.24499403]\nPosition: [0.62044627 0.16388247 0.24499403]\nPosition: [0.62044627 0.16388247 0.24499403]\nPosition: [0.62044592 0.16388242 0.24499403]\nPosition: [0.62044592 0.16388242 0.24499403]\nPosition: [0.62044557 0.16388238 0.24499403]\nPosition: [0.62044557 0.16388238 0.24499403]\nPosition: [0.62044521 0.16388234 0.24499403]\nPosition: [0.62044521 0.16388234 0.24499403]\nPosition: [0.62044486 0.1638823  0.24499403]\nPosition: [0.62044486 0.1638823  0.24499403]\nPosition: [0.62044451 0.16388226 0.24499403]\nPosition: [0.62044451 0.16388226 0.24499403]\nPosition: [0.62044451 0.16388226 0.24499403]\nPosition: [0.62044416 0.16388221 0.24499403]\nPosition: [0.62044416 0.16388221 0.24499403]\nPosition: [0.62044381 0.16388217 0.24499403]\nPosition: [0.62044381 0.16388217 0.24499403]\nPosition: [0.62044346 0.16388213 0.24499403]\nPosition: [0.62044346 0.16388213 0.24499403]\nPosition: [0.62044311 0.16388209 0.24499403]\nPosition: [0.62044311 0.16388209 0.24499403]\nPosition: [0.62044276 0.16388205 0.24499403]\nPosition: [0.62044276 0.16388205 0.24499403]\nPosition: [0.62044242 0.163882   0.24499403]\nPosition: [0.62044242 0.163882   0.24499403]\nPosition: [0.62044207 0.16388196 0.24499403]\nPosition: [0.62044207 0.16388196 0.24499403]\nPosition: [0.62044172 0.16388192 0.24499403]\nPosition: [0.62044172 0.16388192 0.24499403]\nPosition: [0.62044137 0.16388188 0.24499403]\nPosition: [0.62044137 0.16388188 0.24499403]\nPosition: [0.62044103 0.16388184 0.24499403]\nPosition: [0.62044103 0.16388184 0.24499403]\nPosition: [0.62044068 0.16388179 0.24499403]\nPosition: [0.62044068 0.16388179 0.24499403]\nPosition: [0.62044034 0.16388175 0.24499403]\nPosition: [0.62044034 0.16388175 0.24499403]\nPosition: [0.62044    0.16388171 0.24499403]\nPosition: [0.62044    0.16388171 0.24499403]\nPosition: [0.62043965 0.16388167 0.24499403]\nPosition: [0.62043965 0.16388167 0.24499403]\nPosition: [0.62043931 0.16388162 0.24499403]\nPosition: [0.62043931 0.16388162 0.24499403]\nPosition: [0.62043897 0.16388158 0.24499403]\nPosition: [0.62043897 0.16388158 0.24499403]\nPosition: [0.62043863 0.16388154 0.24499403]\nPosition: [0.62043863 0.16388154 0.24499403]\nPosition: [0.62043829 0.1638815  0.24499403]\nPosition: [0.62043829 0.1638815  0.24499403]\nPosition: [0.62043795 0.16388146 0.24499403]\nPosition: [0.62043795 0.16388146 0.24499403]\nPosition: [0.62043761 0.16388141 0.24499403]\nPosition: [0.62043761 0.16388141 0.24499403]\nPosition: [0.62043727 0.16388137 0.24499403]\nPosition: [0.62043727 0.16388137 0.24499403]\nPosition: [0.62043693 0.16388133 0.24499403]\nPosition: [0.62043693 0.16388133 0.24499403]\nPosition: [0.62043659 0.16388129 0.24499403]\nPosition: [0.62043659 0.16388129 0.24499403]\nPosition: [0.62043625 0.16388125 0.24499403]\nPosition: [0.62043625 0.16388125 0.24499403]\nPosition: [0.62043591 0.1638812  0.24499403]\nPosition: [0.62043591 0.1638812  0.24499403]\nPosition: [0.62043558 0.16388116 0.24499403]\nPosition: [0.62043558 0.16388116 0.24499403]\nPosition: [0.62043524 0.16388112 0.24499403]\nPosition: [0.62043524 0.16388112 0.24499403]\nPosition: [0.62043491 0.16388108 0.24499403]\nPosition: [0.62043491 0.16388108 0.24499403]\nPosition: [0.62043457 0.16388103 0.24499403]\nPosition: [0.62043457 0.16388103 0.24499403]\nPosition: [0.62043424 0.16388099 0.24499403]\nPosition: [0.62043424 0.16388099 0.24499403]\nPosition: [0.62043391 0.16388095 0.24499403]\nPosition: [0.62043391 0.16388095 0.24499403]\nPosition: [0.62043357 0.16388091 0.24499403]\nPosition: [0.62043357 0.16388091 0.24499403]\nPosition: [0.62043324 0.16388087 0.24499403]\nPosition: [0.62043324 0.16388087 0.24499403]\nPosition: [0.62043291 0.16388082 0.24499403]\nPosition: [0.62043291 0.16388082 0.24499403]\nPosition: [0.62043258 0.16388078 0.24499403]\nPosition: [0.62043258 0.16388078 0.24499403]\nPosition: [0.62043225 0.16388074 0.24499403]\nPosition: [0.62043225 0.16388074 0.24499403]\nPosition: [0.62043192 0.1638807  0.24499403]\nPosition: [0.62043192 0.1638807  0.24499403]\nPosition: [0.62043159 0.16388065 0.24499403]\nPosition: [0.62043159 0.16388065 0.24499403]\nPosition: [0.62043126 0.16388061 0.24499403]\nPosition: [0.62043126 0.16388061 0.24499403]\nPosition: [0.62043093 0.16388057 0.24499403]\nPosition: [0.62043093 0.16388057 0.24499403]\nPosition: [0.6204306  0.16388053 0.24499403]\nPosition: [0.6204306  0.16388053 0.24499403]\nPosition: [0.62043027 0.16388048 0.24499403]\nPosition: [0.62043027 0.16388048 0.24499403]\nPosition: [0.62042995 0.16388044 0.24499403]\nPosition: [0.62042995 0.16388044 0.24499403]\nPosition: [0.62042962 0.1638804  0.24499403]\nPosition: [0.62042962 0.1638804  0.24499403]\nPosition: [0.6204293  0.16388036 0.24499403]\nPosition: [0.6204293  0.16388036 0.24499403]\nPosition: [0.62042897 0.16388031 0.24499403]\nPosition: [0.62042897 0.16388031 0.24499403]\nPosition: [0.62042865 0.16388027 0.24499403]\nPosition: [0.62042865 0.16388027 0.24499403]\nPosition: [0.62042832 0.16388023 0.24499403]\nPosition: [0.62042832 0.16388023 0.24499403]\nPosition: [0.620428   0.16388019 0.24499403]\nPosition: [0.620428   0.16388019 0.24499403]\nPosition: [0.62042768 0.16388014 0.24499403]\nPosition: [0.62042768 0.16388014 0.24499403]\nPosition: [0.62042735 0.1638801  0.24499403]\nPosition: [0.62042735 0.1638801  0.24499403]\nPosition: [0.62042703 0.16388006 0.24499403]\nPosition: [0.62042703 0.16388006 0.24499403]\nPosition: [0.62042671 0.16388002 0.24499403]\nPosition: [0.62042671 0.16388002 0.24499403]\nPosition: [0.62042639 0.16387998 0.24499403]\nPosition: [0.62042639 0.16387998 0.24499403]\nPosition: [0.62042607 0.16387993 0.24499403]\nPosition: [0.62042607 0.16387993 0.24499403]\nPosition: [0.62042575 0.16387989 0.24499403]\nPosition: [0.62042575 0.16387989 0.24499403]\nPosition: [0.62042543 0.16387985 0.24499403]\nPosition: [0.62042543 0.16387985 0.24499403]\nPosition: [0.62042512 0.16387981 0.24499403]\nPosition: [0.62042512 0.16387981 0.24499403]\nPosition: [0.6204248  0.16387976 0.24499403]\nPosition: [0.6204248  0.16387976 0.24499403]\nPosition: [0.62042448 0.16387972 0.24499403]\nPosition: [0.62042448 0.16387972 0.24499403]\nPosition: [0.62042416 0.16387968 0.24499403]\nPosition: [0.62042416 0.16387968 0.24499403]\nPosition: [0.62042385 0.16387964 0.24499403]\nPosition: [0.62042385 0.16387964 0.24499403]\nPosition: [0.62042353 0.16387959 0.24499403]\nPosition: [0.62042353 0.16387959 0.24499403]\nPosition: [0.62042322 0.16387955 0.24499403]\nPosition: [0.62042322 0.16387955 0.24499403]\nPosition: [0.6204229  0.16387951 0.24499403]\nPosition: [0.6204229  0.16387951 0.24499403]\nPosition: [0.62042259 0.16387947 0.24499403]\nPosition: [0.62042259 0.16387947 0.24499403]\nPosition: [0.62042228 0.16387942 0.24499403]\nPosition: [0.62042228 0.16387942 0.24499403]\nPosition: [0.62042196 0.16387938 0.24499403]\nPosition: [0.62042196 0.16387938 0.24499403]\nPosition: [0.62042165 0.16387934 0.24499403]\nPosition: [0.62042165 0.16387934 0.24499403]\nPosition: [0.62042134 0.1638793  0.24499403]\nPosition: [0.62042134 0.1638793  0.24499403]\nPosition: [0.62042103 0.16387925 0.24499403]\nPosition: [0.62042103 0.16387925 0.24499403]\nPosition: [0.62042072 0.16387921 0.24499403]\nPosition: [0.62042072 0.16387921 0.24499403]\nPosition: [0.62042041 0.16387917 0.24499403]\nPosition: [0.62042041 0.16387917 0.24499403]\nPosition: [0.6204201  0.16387913 0.24499403]\nPosition: [0.6204201  0.16387913 0.24499403]\nPosition: [0.62041979 0.16387908 0.24499403]\nPosition: [0.62041979 0.16387908 0.24499403]\nPosition: [0.62041948 0.16387904 0.24499403]\nPosition: [0.62041948 0.16387904 0.24499403]\nPosition: [0.62041918 0.163879   0.24499403]\nPosition: [0.62041918 0.163879   0.24499403]\nPosition: [0.62041887 0.16387896 0.24499403]\nPosition: [0.62041887 0.16387896 0.24499403]\nPosition: [0.62041887 0.16387896 0.24499403]\nPosition: [0.62041856 0.16387891 0.24499403]\nPosition: [0.62041856 0.16387891 0.24499403]\nPosition: [0.62041826 0.16387887 0.24499403]\nPosition: [0.62041826 0.16387887 0.24499403]\nPosition: [0.62041795 0.16387883 0.24499403]\nPosition: [0.62041795 0.16387883 0.24499403]\nPosition: [0.62041765 0.16387878 0.24499403]\nPosition: [0.62041765 0.16387878 0.24499403]\nPosition: [0.62041734 0.16387874 0.24499403]\nPosition: [0.62041734 0.16387874 0.24499403]\nPosition: [0.62041704 0.1638787  0.24499403]\nPosition: [0.62041704 0.1638787  0.24499403]\nPosition: [0.62041674 0.16387866 0.24499403]\nPosition: [0.62041674 0.16387866 0.24499403]\nPosition: [0.62041643 0.16387861 0.24499403]\nPosition: [0.62041643 0.16387861 0.24499403]\nPosition: [0.62041613 0.16387857 0.24499403]\nPosition: [0.62041613 0.16387857 0.24499403]\nPosition: [0.62041583 0.16387853 0.24499403]\nPosition: [0.62041583 0.16387853 0.24499403]\nPosition: [0.62041553 0.16387849 0.24499403]\nPosition: [0.62041553 0.16387849 0.24499403]\nPosition: [0.62041523 0.16387844 0.24499403]\nPosition: [0.62041523 0.16387844 0.24499403]\nPosition: [0.62041493 0.1638784  0.24499403]\nPosition: [0.62041493 0.1638784  0.24499403]\nPosition: [0.62041463 0.16387836 0.24499403]\nPosition: [0.62041463 0.16387836 0.24499403]\nPosition: [0.62041433 0.16387832 0.24499403]\nPosition: [0.62041433 0.16387832 0.24499403]\nPosition: [0.62041404 0.16387827 0.24499403]\nPosition: [0.62041404 0.16387827 0.24499403]\nPosition: [0.62041374 0.16387823 0.24499403]\nPosition: [0.62041374 0.16387823 0.24499403]\nPosition: [0.62041344 0.16387819 0.24499403]\nPosition: [0.62041344 0.16387819 0.24499403]\nPosition: [0.62041314 0.16387815 0.24499403]\nPosition: [0.62041314 0.16387815 0.24499403]\nPosition: [0.62041285 0.1638781  0.24499403]\nPosition: [0.62041285 0.1638781  0.24499403]\nPosition: [0.62041255 0.16387806 0.24499403]\nPosition: [0.62041255 0.16387806 0.24499403]\nPosition: [0.62041226 0.16387802 0.24499403]\nPosition: [0.62041226 0.16387802 0.24499403]\nPosition: [0.62041196 0.16387797 0.24499403]\nPosition: [0.62041196 0.16387797 0.24499403]\nPosition: [0.62041167 0.16387793 0.24499403]\nPosition: [0.62041167 0.16387793 0.24499403]\nPosition: [0.62041138 0.16387789 0.24499403]\nPosition: [0.62041138 0.16387789 0.24499403]\nPosition: [0.62041109 0.16387785 0.24499403]\nPosition: [0.62041109 0.16387785 0.24499403]\nPosition: [0.62041079 0.1638778  0.24499403]\nPosition: [0.62041079 0.1638778  0.24499403]\nPosition: [0.6204105  0.16387776 0.24499403]\nPosition: [0.6204105  0.16387776 0.24499403]\nPosition: [0.62041021 0.16387772 0.24499403]\nPosition: [0.62041021 0.16387772 0.24499403]\nPosition: [0.62040992 0.16387768 0.24499403]\nPosition: [0.62040992 0.16387768 0.24499403]\nPosition: [0.62040963 0.16387763 0.24499403]\nPosition: [0.62040963 0.16387763 0.24499403]\nPosition: [0.62040934 0.16387759 0.24499403]\nPosition: [0.62040934 0.16387759 0.24499403]\nPosition: [0.62040905 0.16387755 0.24499403]\nPosition: [0.62040905 0.16387755 0.24499403]\nPosition: [0.62040877 0.1638775  0.24499403]\nPosition: [0.62040877 0.1638775  0.24499403]\nPosition: [0.62040848 0.16387746 0.24499403]\nPosition: [0.62040848 0.16387746 0.24499403]\nPosition: [0.62040819 0.16387742 0.24499403]\nPosition: [0.62040819 0.16387742 0.24499403]\nPosition: [0.62040791 0.16387738 0.24499403]\nPosition: [0.62040791 0.16387738 0.24499403]\nPosition: [0.62040762 0.16387733 0.24499403]\nPosition: [0.62040762 0.16387733 0.24499403]\nPosition: [0.62040734 0.16387729 0.24499403]\nPosition: [0.62040734 0.16387729 0.24499403]\nPosition: [0.62040705 0.16387725 0.24499403]\nPosition: [0.62040705 0.16387725 0.24499403]\nPosition: [0.62040677 0.1638772  0.24499403]\nPosition: [0.62040677 0.1638772  0.24499403]\nPosition: [0.62040648 0.16387716 0.24499403]\nPosition: [0.62040648 0.16387716 0.24499403]\nPosition: [0.6204062  0.16387712 0.24499403]\nPosition: [0.6204062  0.16387712 0.24499403]\nPosition: [0.62040592 0.16387708 0.24499403]\nPosition: [0.62040592 0.16387708 0.24499403]\nPosition: [0.62040564 0.16387703 0.24499403]\nPosition: [0.62040564 0.16387703 0.24499403]\nPosition: [0.62040536 0.16387699 0.24499403]\nPosition: [0.62040536 0.16387699 0.24499403]\nPosition: [0.62040507 0.16387695 0.24499403]\nPosition: [0.62040507 0.16387695 0.24499403]\nPosition: [0.62040479 0.1638769  0.24499403]\nPosition: [0.62040479 0.1638769  0.24499403]\nPosition: [0.62040451 0.16387686 0.24499403]\nPosition: [0.62040451 0.16387686 0.24499403]\nPosition: [0.62040424 0.16387682 0.24499403]\nPosition: [0.62040424 0.16387682 0.24499403]\nPosition: [0.62040396 0.16387678 0.24499403]\nPosition: [0.62040396 0.16387678 0.24499403]\nPosition: [0.62040368 0.16387673 0.24499403]\nPosition: [0.62040368 0.16387673 0.24499403]\nPosition: [0.6204034  0.16387669 0.24499403]\nPosition: [0.6204034  0.16387669 0.24499403]\nPosition: [0.62040312 0.16387665 0.24499403]\nPosition: [0.62040312 0.16387665 0.24499403]\nPosition: [0.62040285 0.1638766  0.24499403]\nPosition: [0.62040285 0.1638766  0.24499403]\nPosition: [0.62040257 0.16387656 0.24499403]\nPosition: [0.62040257 0.16387656 0.24499403]\nPosition: [0.6204023  0.16387652 0.24499403]\nPosition: [0.6204023  0.16387652 0.24499403]\nPosition: [0.62040202 0.16387648 0.24499403]\nPosition: [0.62040202 0.16387648 0.24499403]\nPosition: [0.62040175 0.16387643 0.24499403]\nPosition: [0.62040175 0.16387643 0.24499403]\nPosition: [0.62040147 0.16387639 0.24499403]\nPosition: [0.62040147 0.16387639 0.24499403]\nPosition: [0.6204012  0.16387635 0.24499403]\nPosition: [0.6204012  0.16387635 0.24499403]\nPosition: [0.62040093 0.16387631 0.24499403]\nPosition: [0.62040093 0.16387631 0.24499403]\nPosition: [0.62040066 0.16387626 0.24499403]\nPosition: [0.62040066 0.16387626 0.24499403]\nPosition: [0.62040039 0.16387622 0.24499403]\nPosition: [0.62040039 0.16387622 0.24499403]\nPosition: [0.62040011 0.16387618 0.24499403]\nPosition: [0.62040011 0.16387618 0.24499403]\nPosition: [0.62039984 0.16387613 0.24499403]\nPosition: [0.62039984 0.16387613 0.24499403]\nPosition: [0.62039957 0.16387609 0.24499403]\nPosition: [0.62039957 0.16387609 0.24499403]\nPosition: [0.62039931 0.16387605 0.24499403]\nPosition: [0.62039931 0.16387605 0.24499403]\nPosition: [0.62039904 0.163876   0.24499403]\nPosition: [0.62039904 0.163876   0.24499403]\nPosition: [0.62039877 0.16387596 0.24499403]\nPosition: [0.62039877 0.16387596 0.24499403]\nPosition: [0.6203985  0.16387592 0.24499403]\nPosition: [0.6203985  0.16387592 0.24499403]\nPosition: [0.62039823 0.16387588 0.24499403]\nPosition: [0.62039823 0.16387588 0.24499403]\nPosition: [0.62039797 0.16387583 0.24499403]\nPosition: [0.62039797 0.16387583 0.24499403]\nPosition: [0.6203977  0.16387579 0.24499403]\nPosition: [0.6203977  0.16387579 0.24499403]\nPosition: [0.62039744 0.16387575 0.24499403]\nPosition: [0.62039744 0.16387575 0.24499403]\nPosition: [0.62039717 0.1638757  0.24499403]\nPosition: [0.62039717 0.1638757  0.24499403]\nPosition: [0.62039691 0.16387566 0.24499403]\nPosition: [0.62039691 0.16387566 0.24499403]\nPosition: [0.62039664 0.16387562 0.24499403]\nPosition: [0.62039664 0.16387562 0.24499403]\nPosition: [0.62039664 0.16387562 0.24499403]\nPosition: [0.62039638 0.16387558 0.24499403]\nPosition: [0.62039638 0.16387558 0.24499403]\nPosition: [0.62039612 0.16387553 0.24499403]\nPosition: [0.62039612 0.16387553 0.24499403]\nPosition: [0.62039586 0.16387549 0.24499403]\nPosition: [0.62039586 0.16387549 0.24499403]\nPosition: [0.62039559 0.16387545 0.24499403]\nPosition: [0.62039559 0.16387545 0.24499403]\nPosition: [0.62039533 0.1638754  0.24499403]\nPosition: [0.62039533 0.1638754  0.24499403]\nPosition: [0.62039507 0.16387536 0.24499403]\nPosition: [0.62039507 0.16387536 0.24499403]\nPosition: [0.62039481 0.16387532 0.24499403]\nPosition: [0.62039481 0.16387532 0.24499403]\nPosition: [0.62039455 0.16387528 0.24499403]\nPosition: [0.62039455 0.16387528 0.24499403]\nPosition: [0.62039429 0.16387523 0.24499403]\nPosition: [0.62039429 0.16387523 0.24499403]\nPosition: [0.62039404 0.16387519 0.24499403]\nPosition: [0.62039404 0.16387519 0.24499403]\nPosition: [0.62039378 0.16387515 0.24499403]\nPosition: [0.62039378 0.16387515 0.24499403]\nPosition: [0.62039352 0.1638751  0.24499403]\nPosition: [0.62039352 0.1638751  0.24499403]\nPosition: [0.62039326 0.16387506 0.24499403]\nPosition: [0.62039326 0.16387506 0.24499403]\nPosition: [0.62039301 0.16387502 0.24499403]\nPosition: [0.62039301 0.16387502 0.24499403]\nPosition: [0.62039275 0.16387497 0.24499403]\nPosition: [0.62039275 0.16387497 0.24499403]\nPosition: [0.6203925  0.16387493 0.24499403]\nPosition: [0.6203925  0.16387493 0.24499403]\nPosition: [0.62039224 0.16387489 0.24499403]\nPosition: [0.62039224 0.16387489 0.24499403]\nPosition: [0.62039199 0.16387485 0.24499403]\nPosition: [0.62039199 0.16387485 0.24499403]\nPosition: [0.62039174 0.1638748  0.24499403]\nPosition: [0.62039174 0.1638748  0.24499403]\nPosition: [0.62039148 0.16387476 0.24499403]\nPosition: [0.62039148 0.16387476 0.24499403]\nPosition: [0.62039123 0.16387472 0.24499403]\nPosition: [0.62039123 0.16387472 0.24499403]\nPosition: [0.62039098 0.16387467 0.24499403]\nPosition: [0.62039098 0.16387467 0.24499403]\nPosition: [0.62039073 0.16387463 0.24499403]\nPosition: [0.62039073 0.16387463 0.24499403]\nPosition: [0.62039048 0.16387459 0.24499403]\nPosition: [0.62039048 0.16387459 0.24499403]\nPosition: [0.62039023 0.16387455 0.24499403]\nPosition: [0.62039023 0.16387455 0.24499403]\nPosition: [0.62038998 0.1638745  0.24499403]\nPosition: [0.62038998 0.1638745  0.24499403]\nPosition: [0.62038973 0.16387446 0.24499403]\nPosition: [0.62038973 0.16387446 0.24499403]\nPosition: [0.62038948 0.16387442 0.24499403]\nPosition: [0.62038948 0.16387442 0.24499403]\nPosition: [0.62038923 0.16387437 0.24499403]\nPosition: [0.62038923 0.16387437 0.24499403]\nPosition: [0.62038898 0.16387433 0.24499403]\nPosition: [0.62038898 0.16387433 0.24499403]\nPosition: [0.62038874 0.16387429 0.24499403]\nPosition: [0.62038874 0.16387429 0.24499403]\nPosition: [0.62038849 0.16387424 0.24499403]\nPosition: [0.62038849 0.16387424 0.24499403]\nPosition: [0.62038824 0.1638742  0.24499403]\nPosition: [0.62038824 0.1638742  0.24499403]\nPosition: [0.620388   0.16387416 0.24499403]\nPosition: [0.620388   0.16387416 0.24499403]\nPosition: [0.62038775 0.16387412 0.24499403]\nPosition: [0.62038775 0.16387412 0.24499403]\nPosition: [0.62038751 0.16387407 0.24499403]\nPosition: [0.62038751 0.16387407 0.24499403]\nPosition: [0.62038727 0.16387403 0.24499403]\nPosition: [0.62038727 0.16387403 0.24499403]\n",
     "output_type": "stream"
    }
   ],
   "execution_count": null
  },
  {
   "cell_type": "markdown",
   "source": "<a style='text-decoration:none;line-height:16px;display:flex;color:#5B5B62;padding:10px;justify-content:end;' href='https://deepnote.com?utm_source=created-in-deepnote-cell&projectId=e1f7a4de-0698-4427-bcc0-40d4d181e518' target=\"_blank\">\n<img alt='Created in deepnote.com' style='display:inline;max-height:16px;margin:0px;margin-right:7.5px;' src='data:image/svg+xml;base64,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' > </img>\nCreated in <span style='font-weight:600;margin-left:4px;'>Deepnote</span></a>",
   "metadata": {
    "tags": [],
    "created_in_deepnote_cell": true,
    "deepnote_cell_type": "markdown"
   }
  }
 ],
 "nbformat": 4,
 "nbformat_minor": 0,
 "metadata": {
  "colab": {
   "collapsed_sections": [],
   "name": "Robotic Manipulation - Motion Planning.ipynb",
   "provenance": []
  },
  "interpreter": {
   "hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6"
  },
  "kernelspec": {
   "display_name": "Python 3.6.9 64-bit",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  },
  "deepnote_notebook_id": "d416de90-3d3f-4c7b-8038-087e10142c20",
  "deepnote": {},
  "deepnote_execution_queue": [
   {
    "cellId": "00003-de9dbc78-f208-4914-afe1-6bb0eeb6297c",
    "sessionId": "eda20816-05e6-4c84-89f1-03b043b1ec67",
    "msgId": "cf263ffd-06d2-4414-a9ea-e1c8c09c4af6"
   }
  ]
 }
}