diff --git a/python/adapy/ada_bindings.cpp b/python/adapy/ada_bindings.cpp index cd042bd..30a0f2b 100644 --- a/python/adapy/ada_bindings.cpp +++ b/python/adapy/ada_bindings.cpp @@ -140,11 +140,33 @@ 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( @@ -152,19 +174,32 @@ aikido::trajectory::TrajectoryPtr plan_to_offset( 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( + &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_>( m, "AdaHand") diff --git a/python/scripts/simple_trajectories.py b/python/scripts/simple_trajectories.py index 06ba99f..8e41772 100755 --- a/python/scripts/simple_trajectories.py +++ b/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") diff --git a/python/scripts/tsr_example.py b/python/scripts/tsr_example.py index a548355..07a0110 100755 --- a/python/scripts/tsr_example.py +++ b/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")