icaros/py-collision-plan

python/adapy/ada_bindings.cpp

@@ -140,31 +140,66 @@ aikido::trajectory::TrajectoryPtr compute_retime_path(
       self->getSelfCollisionConstraint());
 }
 
+aikido::trajectory::TrajectoryPtr plan_to_configuration(
+    ada::Ada* self,
+    const Eigen::VectorXd& configuration,
+    aikido::constraint::TestablePtr& testableConstraint)
+{
+  auto traj
+      = self->getArm()->planToConfiguration(configuration, testableConstraint);
+  return traj;
+}
+
 aikido::trajectory::TrajectoryPtr plan_to_configuration(
     ada::Ada* self, const Eigen::VectorXd& configuration)
 {
-  auto trajectory = self->getArm()->planToConfiguration(configuration);
-  return trajectory;
+  // Default to self collision constraint
+  auto traj = self->getArm()->planToConfiguration(configuration);
+  return traj;
+}
+
+aikido::trajectory::TrajectoryPtr plan_to_offset(
+    ada::Ada* self,
+    const std::string bodyNodeName,
+    const Eigen::Vector3d& offset,
+    aikido::constraint::TestablePtr& testableConstraint)
+{
+  auto traj
+      = self->getArm()->planToOffset(bodyNodeName, offset, testableConstraint);
+  return traj;
 }
 
 aikido::trajectory::TrajectoryPtr plan_to_offset(
     ada::Ada* self,
     const std::string bodyNodeName,
     const Eigen::Vector3d& offset)
 {
+  // Default to self collision constraint
   auto trajectory = self->getArm()->planToOffset(bodyNodeName, offset);
   return trajectory;
 }
 
+aikido::trajectory::TrajectoryPtr plan_to_tsr(
+    ada::Ada* self,
+    const std::string bodyNodeName,
+    const aikido::constraint::dart::TSRPtr& tsr,
+    aikido::constraint::TestablePtr& testableConstraint)
+{
+  auto traj = self->getArm()->planToTSR(bodyNodeName, tsr, testableConstraint);
+  return traj;
+}
+
 aikido::trajectory::TrajectoryPtr plan_to_tsr(
     ada::Ada* self,
     const std::string bodyNodeName,
     const aikido::constraint::dart::TSRPtr& tsr)
 {
-  auto trajectory = self->getArm()->planToTSR(bodyNodeName, tsr);
-  return trajectory;
+  // Default to self collision constraint
+  auto traj = self->getArm()->planToTSR(bodyNodeName, tsr);
+  return traj;
 }
 
+
 void execute_trajectory(
     ada::Ada* self, const aikido::trajectory::TrajectoryPtr& trajectory)
 {
@@ -293,11 +328,46 @@ void Ada(pybind11::module& m)
       .def("compute_joint_space_path", compute_joint_space_path)
       .def("compute_smooth_joint_space_path", compute_smooth_joint_space_path)
       .def("compute_retime_path", compute_retime_path)
-      .def("plan_to_configuration", plan_to_configuration)
-      .def("plan_to_offset", plan_to_offset)
-      .def("plan_to_tsr", plan_to_tsr)
       .def("execute_trajectory", execute_trajectory)
       .def("start_viewer", start_viewer);
+
+      // overload plan_to_ methods to make collision constraint optional
+      .def(
+          "plan_to_configuration",
+          py::overload_cast<ada::Ada*, const Eigen::VectorXd&>(
+              &plan_to_configuration))
+      .def(
+          "plan_to_configuration",
+          py::overload_cast<
+              ada::Ada*,
+              const Eigen::VectorXd&,
+              aikido::constraint::TestablePtr&>(&plan_to_configuration))
+      .def(
+          "plan_to_offset",
+          py::overload_cast<
+              ada::Ada*,
+              const std::string,
+              const Eigen::Vector3d&>(&plan_to_offset))
+      .def(
+          "plan_to_offset",
+          py::overload_cast<
+              ada::Ada*,
+              const std::string,
+              const Eigen::Vector3d&,
+              aikido::constraint::TestablePtr&>(&plan_to_offset))
+      .def(
+          "plan_to_tsr",
+          py::overload_cast<
+              ada::Ada*,
+              const std::string,
+              const aikido::constraint::dart::TSRPtr&>(&plan_to_tsr))
+      .def(
+          "plan_to_tsr",
+          py::overload_cast<
+              ada::Ada*,
+              const std::string,
+              const aikido::constraint::dart::TSRPtr&,
+              aikido::constraint::TestablePtr&>(&plan_to_tsr))
 
   py::class_<ada::Ada::AdaHand, std::shared_ptr<ada::Ada::AdaHand>>(
       m, "AdaHand")

python/scripts/simple_trajectories.py

@@ -23,8 +23,10 @@
     viewer = ada.start_viewer("dart_markers/simple_trajectories", "map")
     canURDFUri = "package://pr_assets/data/objects/can.urdf"
     sodaCanPose = [1.0, 0.0, 0.73, 0, 0, 0, 1]
-    tableURDFUri = "package://pr_assets/data/furniture/uw_demo_table.urdf"
-    tablePose = [1., 0.0, 0.0, 0.707107, 0, 0, 0.707107]
+    tableURDFUri = "package://pr_assets/data/objects/large_white_box.urdf"
+    tablePose = [0.45, -0.15, 0.55, 0.707107, 0, 0, 0.707107]
+    # tableURDFUri = "package://pr_assets/data/furniture/uw_demo_table.urdf"
+    # tablePose = [0.73, 0.73, 0., 0.707107, 0, 0, 0.707107]
     world = ada.get_world()
     can = world.add_body_from_urdf(canURDFUri, sodaCanPose)
     table = world.add_body_from_urdf(tableURDFUri, tablePose)
@@ -40,12 +42,22 @@
     positions2[2] += 0.2
     positions3[0] += 1.5
     positions3[2] += 0.4
+    positions4[0] += 0.8
     positions4[1] -= 0.4
-    positions4[2] += 0.6
+    positions4[2] += 1.4
 
     waypoints = [(0.0, positions), (1.0, positions2), (2.0, positions3), (3.0, positions4)]
     waypoints_rev = [(0.0, positions4), (1.0, positions3), (2.0, positions2), (3.0, positions)]
-    traj = ada.plan_to_configuration(positions4)
+
+    print("")
+    print("CONTINUE TO CONFIGURATION")
+    print("")
+    traj = ada.plan_to_configuration(positions4, collision)
+    if not traj:
+        print("")
+        print("Did not find collision free path!")
+        traj = ada.plan_to_configuration(positions4)
+
     traj_rev = ada.compute_joint_space_path(waypoints_rev)
 
     print("")
@@ -58,8 +70,8 @@
     offset = [0., 0.1, 0.]
     offset_rev = [0., -0.1, 0.]
     hand_node = rospy.get_param("adaConf/hand_base")
-    traj_off = ada.plan_to_offset(hand_node, offset)
-    traj_off_rev = ada.plan_to_offset(hand_node, offset_rev)
+    traj_off = ada.plan_to_offset(hand_node, offset, collision)
+    traj_off_rev = ada.plan_to_offset(hand_node, offset_rev, collision)
 
     print("")
     print("CONTINUE TO OFFSET")

python/scripts/tsr_example.py

@@ -24,8 +24,11 @@
     viewer = ada.start_viewer("dart_markers/tsr_example", "map")
     canURDFUri = "package://pr_assets/data/objects/can.urdf"
     sodaCanPose = [0.2, 0.2, 0., 0, 0, 0, 1]
+    tableURDFUri = "package://pr_assets/data/furniture/uw_demo_table.urdf"
+    tablePose = [0.8, 0.0, 0.0, 0.707107, 0, 0, 0.707107]
     world = ada.get_world()
     can = world.add_body_from_urdf(canURDFUri, sodaCanPose)
+    table = world.add_body_from_urdf(tableURDFUri, tablePose)
 
     collision = ada.get_self_collision_constraint()
 
@@ -82,25 +85,26 @@
     print("Tw_e", grasp_tsr.get_Tw_e())
 
     print("")
-    print("PLAN TO TSR")
+    print("PLAN TO TSR with COLLISION")
     print("")
     pdb.set_trace()
-
+     
     trials, maxTrials = 0, 10
     while trials < maxTrials:
 
         # plan_to_tsr may require more than one try to find a succesful path
-        traj = ada.plan_to_tsr(rospy.get_param("adaConf/end_effector"), grasp_tsr)
+        traj = ada.plan_to_tsr(rospy.get_param("adaConf/end_effector"), grasp_tsr, ada.get_world_collision_constraint())
         if traj:
             break
         trials += 1
 
-    print("")
-    print("CONTINUE TO EXECUTE")
-    print("")
-    pdb.set_trace()
+    if traj:
+        ada.execute_trajectory(traj)
+    else:
+        print("")
+        print("DID NOT FIND A COLLISION-FREE PATH!")
+        print("")
 
-    ada.execute_trajectory(traj)
 
     print("")
     print("DONE! CONTINUE TO EXIT")