From 701dd0d9a6d8fe1a0fa8c03db3052ac843b04661 Mon Sep 17 00:00:00 2001 From: hsnemlekar Date: Thu, 2 Jun 2022 10:32:21 -0700 Subject: [PATCH 1/7] added constraint to python planners --- python/adapy/ada_bindings.cpp | 15 +++++++++------ python/scripts/simple_trajectories.py | 6 +++--- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/python/adapy/ada_bindings.cpp b/python/adapy/ada_bindings.cpp index cd042bd..0851303 100644 --- a/python/adapy/ada_bindings.cpp +++ b/python/adapy/ada_bindings.cpp @@ -141,27 +141,30 @@ aikido::trajectory::TrajectoryPtr compute_retime_path( } aikido::trajectory::TrajectoryPtr plan_to_configuration( - ada::Ada* self, const Eigen::VectorXd& configuration) + ada::Ada* self, const Eigen::VectorXd& configuration, + const aikido::constraint::TestablePtr& testableConstraint) { - auto trajectory = self->getArm()->planToConfiguration(configuration); + auto trajectory = self->getArm()->planToConfiguration(configuration, testableConstraint); return trajectory; } aikido::trajectory::TrajectoryPtr plan_to_offset( ada::Ada* self, const std::string bodyNodeName, - const Eigen::Vector3d& offset) + const Eigen::Vector3d& offset, + const aikido::constraint::TestablePtr& testableConstraint) { - auto trajectory = self->getArm()->planToOffset(bodyNodeName, offset); + auto trajectory = self->getArm()->planToOffset(bodyNodeName, offset, testableConstraint); return trajectory; } aikido::trajectory::TrajectoryPtr plan_to_tsr( ada::Ada* self, const std::string bodyNodeName, - const aikido::constraint::dart::TSRPtr& tsr) + const aikido::constraint::dart::TSRPtr& tsr, + const aikido::constraint::TestablePtr& testableConstraint) { - auto trajectory = self->getArm()->planToTSR(bodyNodeName, tsr); + auto trajectory = self->getArm()->planToTSR(bodyNodeName, tsr, testableConstraint); return trajectory; } diff --git a/python/scripts/simple_trajectories.py b/python/scripts/simple_trajectories.py index 06ba99f..5264858 100755 --- a/python/scripts/simple_trajectories.py +++ b/python/scripts/simple_trajectories.py @@ -45,7 +45,7 @@ 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) + traj = ada.plan_to_configuration(positions4, collision) traj_rev = ada.compute_joint_space_path(waypoints_rev) print("") @@ -58,8 +58,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") From 5d99a7e657e66345adcf858387e838f01ee86c25 Mon Sep 17 00:00:00 2001 From: hsnemlekar Date: Thu, 2 Jun 2022 13:36:04 -0700 Subject: [PATCH 2/7] overload plan_to_configuration --- python/adapy/ada_bindings.cpp | 18 ++++++++++++++---- python/scripts/simple_trajectories.py | 2 +- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/python/adapy/ada_bindings.cpp b/python/adapy/ada_bindings.cpp index 0851303..7869441 100644 --- a/python/adapy/ada_bindings.cpp +++ b/python/adapy/ada_bindings.cpp @@ -140,12 +140,21 @@ aikido::trajectory::TrajectoryPtr compute_retime_path( self->getSelfCollisionConstraint()); } +aikido::trajectory::TrajectoryPtr plan_to_configuration( + ada::Ada* self, const Eigen::VectorXd& configuration) +{ + auto traj = self->getArm()->planToConfiguration(configuration); + return traj; + +} + aikido::trajectory::TrajectoryPtr plan_to_configuration( ada::Ada* self, const Eigen::VectorXd& configuration, - const aikido::constraint::TestablePtr& testableConstraint) + aikido::constraint::TestablePtr& testableConstraint) { - auto trajectory = self->getArm()->planToConfiguration(configuration, testableConstraint); - return trajectory; + auto traj = self->getArm()->planToConfiguration(configuration, testableConstraint); + return traj; + } aikido::trajectory::TrajectoryPtr plan_to_offset( @@ -296,7 +305,8 @@ 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_configuration", py::overload_cast(&plan_to_configuration)) + .def("plan_to_configuration", py::overload_cast(&plan_to_configuration)) .def("plan_to_offset", plan_to_offset) .def("plan_to_tsr", plan_to_tsr) .def("execute_trajectory", execute_trajectory) diff --git a/python/scripts/simple_trajectories.py b/python/scripts/simple_trajectories.py index 5264858..b7c5f3d 100755 --- a/python/scripts/simple_trajectories.py +++ b/python/scripts/simple_trajectories.py @@ -45,7 +45,7 @@ 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, collision) + traj = ada.plan_to_configuration(positions4) traj_rev = ada.compute_joint_space_path(waypoints_rev) print("") From 6cd50ca768d746c82939bb3e8aeebf5f6e2251d6 Mon Sep 17 00:00:00 2001 From: hsnemlekar Date: Thu, 2 Jun 2022 14:26:56 -0700 Subject: [PATCH 3/7] added overloading for plan to offset and tsr --- python/adapy/ada_bindings.cpp | 36 +++++++++++++++++++++------ python/scripts/simple_trajectories.py | 6 ++--- 2 files changed, 31 insertions(+), 11 deletions(-) diff --git a/python/adapy/ada_bindings.cpp b/python/adapy/ada_bindings.cpp index 7869441..bca23d2 100644 --- a/python/adapy/ada_bindings.cpp +++ b/python/adapy/ada_bindings.cpp @@ -160,21 +160,39 @@ aikido::trajectory::TrajectoryPtr plan_to_configuration( aikido::trajectory::TrajectoryPtr plan_to_offset( ada::Ada* self, const std::string bodyNodeName, - const Eigen::Vector3d& offset, - const aikido::constraint::TestablePtr& testableConstraint) + const Eigen::Vector3d& offset) { - auto trajectory = self->getArm()->planToOffset(bodyNodeName, offset, testableConstraint); + auto trajectory = self->getArm()->planToOffset(bodyNodeName, offset); return trajectory; } +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_tsr( + ada::Ada* self, + const std::string bodyNodeName, + const aikido::constraint::dart::TSRPtr& tsr) +{ + auto traj = self->getArm()->planToTSR(bodyNodeName, tsr); + return traj; +} + aikido::trajectory::TrajectoryPtr plan_to_tsr( ada::Ada* self, const std::string bodyNodeName, const aikido::constraint::dart::TSRPtr& tsr, - const aikido::constraint::TestablePtr& testableConstraint) + aikido::constraint::TestablePtr& testableConstraint) { - auto trajectory = self->getArm()->planToTSR(bodyNodeName, tsr, testableConstraint); - return trajectory; + auto traj = self->getArm()->planToTSR(bodyNodeName, tsr, testableConstraint); + return traj; } void execute_trajectory( @@ -307,8 +325,10 @@ void Ada(pybind11::module& m) .def("compute_retime_path", compute_retime_path) .def("plan_to_configuration", py::overload_cast(&plan_to_configuration)) .def("plan_to_configuration", py::overload_cast(&plan_to_configuration)) - .def("plan_to_offset", plan_to_offset) - .def("plan_to_tsr", plan_to_tsr) + .def("plan_to_offset", py::overload_cast(&plan_to_offset)) + .def("plan_to_offset", py::overload_cast(&plan_to_offset)) + .def("plan_to_tsr", py::overload_cast(&plan_to_tsr)) + .def("plan_to_tsr", py::overload_cast(&plan_to_tsr)) .def("execute_trajectory", execute_trajectory) .def("start_viewer", start_viewer); diff --git a/python/scripts/simple_trajectories.py b/python/scripts/simple_trajectories.py index b7c5f3d..f7896ce 100755 --- a/python/scripts/simple_trajectories.py +++ b/python/scripts/simple_trajectories.py @@ -45,7 +45,7 @@ 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) + traj = ada.plan_to_configuration(positions4) traj_rev = ada.compute_joint_space_path(waypoints_rev) print("") @@ -58,8 +58,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, collision) - traj_off_rev = ada.plan_to_offset(hand_node, offset_rev, collision) + traj_off = ada.plan_to_offset(hand_node, offset) + traj_off_rev = ada.plan_to_offset(hand_node, offset_rev) print("") print("CONTINUE TO OFFSET") From 51529e075a072d28f9ac3af1d62619f6024ec68e Mon Sep 17 00:00:00 2001 From: hsnemlekar Date: Thu, 2 Jun 2022 14:32:43 -0700 Subject: [PATCH 4/7] format code --- python/adapy/ada_bindings.cpp | 53 +++++++++++++++++++++++++++-------- 1 file changed, 42 insertions(+), 11 deletions(-) diff --git a/python/adapy/ada_bindings.cpp b/python/adapy/ada_bindings.cpp index bca23d2..7ccaf31 100644 --- a/python/adapy/ada_bindings.cpp +++ b/python/adapy/ada_bindings.cpp @@ -145,16 +145,16 @@ aikido::trajectory::TrajectoryPtr plan_to_configuration( { auto traj = self->getArm()->planToConfiguration(configuration); return traj; - } aikido::trajectory::TrajectoryPtr plan_to_configuration( - ada::Ada* self, const Eigen::VectorXd& configuration, + ada::Ada* self, + const Eigen::VectorXd& configuration, aikido::constraint::TestablePtr& testableConstraint) { - auto traj = self->getArm()->planToConfiguration(configuration, testableConstraint); + auto traj + = self->getArm()->planToConfiguration(configuration, testableConstraint); return traj; - } aikido::trajectory::TrajectoryPtr plan_to_offset( @@ -172,7 +172,8 @@ aikido::trajectory::TrajectoryPtr plan_to_offset( const Eigen::Vector3d& offset, aikido::constraint::TestablePtr& testableConstraint) { - auto traj = self->getArm()->planToOffset(bodyNodeName, offset, testableConstraint); + auto traj + = self->getArm()->planToOffset(bodyNodeName, offset, testableConstraint); return traj; } @@ -323,12 +324,42 @@ 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", py::overload_cast(&plan_to_configuration)) - .def("plan_to_configuration", py::overload_cast(&plan_to_configuration)) - .def("plan_to_offset", py::overload_cast(&plan_to_offset)) - .def("plan_to_offset", py::overload_cast(&plan_to_offset)) - .def("plan_to_tsr", py::overload_cast(&plan_to_tsr)) - .def("plan_to_tsr", py::overload_cast(&plan_to_tsr)) + .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)) .def("execute_trajectory", execute_trajectory) .def("start_viewer", start_viewer); From 0f06b2756df577bf814f0ef5bd9d7646427a03c5 Mon Sep 17 00:00:00 2001 From: hsnemlekar Date: Thu, 4 Aug 2022 14:14:32 -0700 Subject: [PATCH 5/7] added comments to new methods --- python/adapy/ada_bindings.cpp | 42 +++++++++++++++------------ python/scripts/simple_trajectories.py | 6 ++-- python/scripts/tsr_example.py | 2 +- 3 files changed, 28 insertions(+), 22 deletions(-) diff --git a/python/adapy/ada_bindings.cpp b/python/adapy/ada_bindings.cpp index 7ccaf31..30a0f2b 100644 --- a/python/adapy/ada_bindings.cpp +++ b/python/adapy/ada_bindings.cpp @@ -140,20 +140,32 @@ 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) { + // Default to self collision constraint auto traj = self->getArm()->planToConfiguration(configuration); return traj; } -aikido::trajectory::TrajectoryPtr plan_to_configuration( +aikido::trajectory::TrajectoryPtr plan_to_offset( ada::Ada* self, - const Eigen::VectorXd& configuration, + const std::string bodyNodeName, + const Eigen::Vector3d& offset, aikido::constraint::TestablePtr& testableConstraint) { auto traj - = self->getArm()->planToConfiguration(configuration, testableConstraint); + = self->getArm()->planToOffset(bodyNodeName, offset, testableConstraint); return traj; } @@ -162,18 +174,18 @@ 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_offset( +aikido::trajectory::TrajectoryPtr plan_to_tsr( ada::Ada* self, const std::string bodyNodeName, - const Eigen::Vector3d& offset, + const aikido::constraint::dart::TSRPtr& tsr, aikido::constraint::TestablePtr& testableConstraint) { - auto traj - = self->getArm()->planToOffset(bodyNodeName, offset, testableConstraint); + auto traj = self->getArm()->planToTSR(bodyNodeName, tsr, testableConstraint); return traj; } @@ -182,19 +194,11 @@ aikido::trajectory::TrajectoryPtr plan_to_tsr( const std::string bodyNodeName, const aikido::constraint::dart::TSRPtr& tsr) { + // Default to self collision constraint auto traj = self->getArm()->planToTSR(bodyNodeName, tsr); return traj; } -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; -} void execute_trajectory( ada::Ada* self, const aikido::trajectory::TrajectoryPtr& trajectory) @@ -324,6 +328,10 @@ 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("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( @@ -360,8 +368,6 @@ void Ada(pybind11::module& m) const std::string, const aikido::constraint::dart::TSRPtr&, aikido::constraint::TestablePtr&>(&plan_to_tsr)) - .def("execute_trajectory", execute_trajectory) - .def("start_viewer", start_viewer); py::class_>( m, "AdaHand") diff --git a/python/scripts/simple_trajectories.py b/python/scripts/simple_trajectories.py index f7896ce..e2957f5 100755 --- a/python/scripts/simple_trajectories.py +++ b/python/scripts/simple_trajectories.py @@ -45,7 +45,7 @@ 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) + traj = ada.plan_to_configuration(positions4, ada.get_world_collision_constraint()) traj_rev = ada.compute_joint_space_path(waypoints_rev) print("") @@ -58,8 +58,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, ada.get_world_collision_constraint()) + traj_off_rev = ada.plan_to_offset(hand_node, offset_rev, ada.get_world_collision_constraint()) print("") print("CONTINUE TO OFFSET") diff --git a/python/scripts/tsr_example.py b/python/scripts/tsr_example.py index a548355..f241413 100755 --- a/python/scripts/tsr_example.py +++ b/python/scripts/tsr_example.py @@ -90,7 +90,7 @@ 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 From 6ff14ace5f44efab5d36b9d33d273622f83d57be Mon Sep 17 00:00:00 2001 From: hsnemlekar Date: Thu, 4 Aug 2022 14:52:53 -0700 Subject: [PATCH 6/7] updated test scripts with collision constraint --- python/scripts/simple_trajectories.py | 19 ++++++++++++++----- python/scripts/tsr_example.py | 22 +++++++++++++++++++++- 2 files changed, 35 insertions(+), 6 deletions(-) diff --git a/python/scripts/simple_trajectories.py b/python/scripts/simple_trajectories.py index e2957f5..3e2a0d1 100755 --- a/python/scripts/simple_trajectories.py +++ b/python/scripts/simple_trajectories.py @@ -24,7 +24,7 @@ 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] + tablePose = [0.85, 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) @@ -41,11 +41,20 @@ positions3[0] += 1.5 positions3[2] += 0.4 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, ada.get_world_collision_constraint()) + + 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 +67,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, ada.get_world_collision_constraint()) - traj_off_rev = ada.plan_to_offset(hand_node, offset_rev, ada.get_world_collision_constraint()) + 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 f241413..a2c0704 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,7 +85,7 @@ print("Tw_e", grasp_tsr.get_Tw_e()) print("") - print("PLAN TO TSR") + print("PLAN TO TSR with COLLISION") print("") pdb.set_trace() @@ -95,6 +98,23 @@ break trials += 1 + if not traj: + print("Did not find collision free path!") + + print("") + print("PLAN TO TSR without 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) + if traj: + break + trials += 1 + print("") print("CONTINUE TO EXECUTE") print("") From bf83aa03322935ce672645e828c92d98be09c7e3 Mon Sep 17 00:00:00 2001 From: hsnemlekar Date: Thu, 6 Oct 2022 12:23:02 -0700 Subject: [PATCH 7/7] changed environment to test collision avoidance --- python/scripts/simple_trajectories.py | 7 +++++-- python/scripts/tsr_example.py | 26 +++++--------------------- 2 files changed, 10 insertions(+), 23 deletions(-) diff --git a/python/scripts/simple_trajectories.py b/python/scripts/simple_trajectories.py index 3e2a0d1..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 = [0.85, 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,6 +42,7 @@ positions2[2] += 0.2 positions3[0] += 1.5 positions3[2] += 0.4 + positions4[0] += 0.8 positions4[1] -= 0.4 positions4[2] += 1.4 diff --git a/python/scripts/tsr_example.py b/python/scripts/tsr_example.py index a2c0704..07a0110 100755 --- a/python/scripts/tsr_example.py +++ b/python/scripts/tsr_example.py @@ -88,7 +88,7 @@ print("PLAN TO TSR with COLLISION") print("") pdb.set_trace() - + trials, maxTrials = 0, 10 while trials < maxTrials: @@ -98,29 +98,13 @@ break trials += 1 - if not traj: - print("Did not find collision free path!") - + if traj: + ada.execute_trajectory(traj) + else: print("") - print("PLAN TO TSR without COLLISION") + print("DID NOT FIND A COLLISION-FREE PATH!") 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) - if traj: - break - trials += 1 - - print("") - print("CONTINUE TO EXECUTE") - print("") - pdb.set_trace() - ada.execute_trajectory(traj) print("") print("DONE! CONTINUE TO EXIT")