From 7e76de5d193f17d012a13f90e35891282e9ff3bc Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Wed, 24 Feb 2021 15:33:21 -0800 Subject: [PATCH 01/38] [WIP] Add ICAROS's adapy --- CMakeLists.txt | 52 +++++- src/adapy/ada_py.cpp | 173 ++++++++++++++++++ src/adapy/adapy.cpp | 24 +++ src/adapy/aikido_py.cpp | 128 +++++++++++++ src/adapy/dart_py.cpp | 40 ++++ src/adapy/ik_py.cpp | 55 ++++++ src/adapy/pr_tsr_py.cpp | 37 ++++ src/{ => libada}/Ada.cpp | 0 ...maticSimulationPositionCommandExecutor.cpp | 0 src/{ => libada}/AdaHand.cpp | 0 ...maticSimulationPositionCommandExecutor.cpp | 0 src/{ => libada}/util.cpp | 0 src/scripts/simple_trajectories.py | 43 +++++ src/scripts/test_collisions.py | 56 ++++++ 14 files changed, 600 insertions(+), 8 deletions(-) create mode 100644 src/adapy/ada_py.cpp create mode 100644 src/adapy/adapy.cpp create mode 100644 src/adapy/aikido_py.cpp create mode 100644 src/adapy/dart_py.cpp create mode 100644 src/adapy/ik_py.cpp create mode 100644 src/adapy/pr_tsr_py.cpp rename src/{ => libada}/Ada.cpp (100%) rename src/{ => libada}/AdaFingerKinematicSimulationPositionCommandExecutor.cpp (100%) rename src/{ => libada}/AdaHand.cpp (100%) rename src/{ => libada}/AdaHandKinematicSimulationPositionCommandExecutor.cpp (100%) rename src/{ => libada}/util.cpp (100%) create mode 100755 src/scripts/simple_trajectories.py create mode 100644 src/scripts/test_collisions.py diff --git a/CMakeLists.txt b/CMakeLists.txt index b5a9f38..eb70679 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,12 @@ cmake_minimum_required(VERSION 3.10.0) project(libada) -set(library_VERSION 0.2.0) +set(library_VERSION 0.3.0) + +if(NOT ADAPY_PYTHON_VERSION) + set(ADAPY_PYTHON_VERSION 3.8) +endif() + +find_package(PythonInterp ${ADAPY_PYTHON_VERSION} REQUIRED) option(LIBADA_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF) @@ -27,7 +33,7 @@ endif() include(ExternalProject) find_package(DART 6.8.5 REQUIRED - COMPONENTS utils utils-urdf optimizer-nlopt + COMPONENTS utils utils-urdf optimizer-nlopt rviz ) @@ -49,9 +55,11 @@ find_package(aikido 0.3.0 REQUIRED io ) +find_package(PythonLibs 3.8 REQUIRED) + find_package(ada_description REQUIRED) -find_package(Boost REQUIRED) +find_package(Boost REQUIRED python) find_package(controller_manager_msgs REQUIRED) @@ -75,11 +83,17 @@ endif() # set(sources - src/AdaFingerKinematicSimulationPositionCommandExecutor.cpp - src/AdaHandKinematicSimulationPositionCommandExecutor.cpp - src/AdaHand.cpp - src/Ada.cpp - src/util.cpp + src/libada/AdaFingerKinematicSimulationPositionCommandExecutor.cpp + src/libada/AdaHandKinematicSimulationPositionCommandExecutor.cpp + src/libada/AdaHand.cpp + src/libada/Ada.cpp + src/adapy/adapy.cpp + src/adapy/ada_py.cpp + src/adapy/aikido_py.cpp + src/adapy/dart_py.cpp + src/adapy/pr_tsr_py.cpp + src/adapy/ik_py.cpp + src/libada/util.cpp ) add_library(libada SHARED ${sources}) @@ -116,6 +130,25 @@ target_link_libraries(libada ${urdf_LIBRARIES} ) +#============================================================================= +#Pybind + +pybind11_add_module(adapy + MODULE + src/adapy/adapy.cpp + src/adapy/ada_py.cpp + src/adapy/aikido_py.cpp + src/adapy/dart_py.cpp + src/adapy/pr_tsr_py.cpp + src/adapy/ik_py.cpp +) + +target_link_libraries(adapy PUBLIC + libada + ${libaikidopy_LIBRARIES} + ${DART_LIBRARIES} +) + #================================================================================ # Testing # @@ -153,6 +186,9 @@ install(TARGETS libada EXPORT libadaConfig RUNTIME DESTINATION bin) install(DIRECTORY include/ DESTINATION include) +install(TARGETS adapy + LIBRARY DESTINATION lib/python3.8/dist-packages) + # This makes the project importable from the install directory install(EXPORT libadaConfig DESTINATION share/libada/cmake) diff --git a/src/adapy/ada_py.cpp b/src/adapy/ada_py.cpp new file mode 100644 index 0000000..5a3348a --- /dev/null +++ b/src/adapy/ada_py.cpp @@ -0,0 +1,173 @@ +#include +#include +#include +#include +#include +#include + +#include "libada/Ada.hpp" +#include "aikido/statespace/ScopedState.hpp" +#include +namespace py = pybind11; + +void Ada(pybind11::module& m) { + //====================================LIBADA========================================================== + py::class_>(m, "Ada") + .def(py::init([](bool simulation) -> std::shared_ptr { + return std::make_shared( + aikido::planner::World::create(), simulation); + })) + .def("get_self_collision_constraint", + [](ada::Ada *self) -> std::shared_ptr { + return self->getSelfCollisionConstraint( + self->getStateSpace(), + self->getMetaSkeleton() + ); + }) + .def("start_trajectory_executor", [](ada::Ada *self) -> void { + self->startTrajectoryExecutor(); + }) + .def("get_name", + [](ada::Ada *self) -> std::string { return self->getName(); }) + .def("get_world", + [](ada::Ada *self) -> aikido::planner::WorldPtr { + return self->getWorld(); + }) + .def("get_hand", + [](ada::Ada *self) -> ada::AdaHandPtr { + return self->getHand(); + }) + .def("get_skeleton", [](ada::Ada *self) -> dart::dynamics::MetaSkeletonPtr { + return self->getMetaSkeleton(); + }) + .def("get_arm_skeleton", [](ada::Ada *self) -> dart::dynamics::MetaSkeletonPtr { + return self->getArm()->getMetaSkeleton(); + }) + .def("get_arm_state_space", [](ada::Ada *self) -> aikido::statespace::dart::MetaSkeletonStateSpacePtr { + return self->getArm()->getStateSpace(); + }) + .def("set_positions",[](ada::Ada *self,const Eigen::VectorXd& configuration) -> void { + auto arm = self->getArm(); + auto armSkeleton = arm->getMetaSkeleton(); + armSkeleton->setPositions(configuration); + }) + .def("set_up_collision_detection", + [](ada::Ada *self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr &armSpace, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + const std::vector& envSkeletons) -> std::shared_ptr { + dart::collision::FCLCollisionDetectorPtr collisionDetector = dart::collision::FCLCollisionDetector::create(); + std::shared_ptr armCollisionGroup = + collisionDetector->createCollisionGroup(self->getMetaSkeleton().get()); + std::shared_ptr envCollisionGroup = + collisionDetector->createCollisionGroup(); + for (const auto& envSkeleton : envSkeletons) { + envCollisionGroup->addShapeFramesOf(envSkeleton.get()); + } + std::shared_ptr collisionFreeConstraint = + std::make_shared(armSpace, + armSkeleton, + collisionDetector); + collisionFreeConstraint->addPairwiseCheck(armCollisionGroup, envCollisionGroup); + return collisionFreeConstraint; + }) + .def("get_full_collision_constraint", + [](ada::Ada *self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr &armSpace, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + const aikido::constraint::dart::CollisionFreePtr& collisionFreeConstraint) -> aikido::constraint::TestablePtr { + return self->getFullCollisionConstraint(armSpace, armSkeleton, collisionFreeConstraint); + }) + .def("compute_joint_space_path", + [](ada::Ada *self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr &stateSpace, + const std::vector> &waypoints) + -> aikido::trajectory::TrajectoryPtr { + return self->computeJointSpacePath(stateSpace, waypoints); + }) + .def("compute_smooth_joint_space_path", + [](ada::Ada *self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr &stateSpace, + const std::vector> &waypoints, + const aikido::constraint::dart::CollisionFreePtr &collisionFreeConstraint=nullptr) + -> aikido::trajectory::TrajectoryPtr { + return self->computeSmoothJointSpacePath(stateSpace, waypoints, collisionFreeConstraint); + }) + .def("compute_retime_path", + [](ada::Ada *self, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + aikido::trajectory::TrajectoryPtr trajectory_ptr) -> aikido::trajectory::TrajectoryPtr { + return self->retimePath(armSkeleton, trajectory_ptr.get()); + }) + .def("plan_to_configuration", + [](ada::Ada *self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr &armSpace, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + const Eigen::VectorXd& configuration) -> aikido::trajectory::TrajectoryPtr { + auto state = armSpace->createState(); + armSpace->convertPositionsToState(configuration, state); + auto trajectory = self->planToConfiguration(armSpace, + armSkeleton, + state, + nullptr, + 10); + return trajectory; + }) + .def("execute_trajectory", + [](ada::Ada *self, + const aikido::trajectory::TrajectoryPtr &trajectory) + -> void { + auto future = self->executeTrajectory(trajectory); + +// if (!future.valid()) { +// std::__throw_future_error(0); +// } +// +// future.wait(); +// ros::Duration(15).sleep(); + }) + .def("start_viewer", + [](ada::Ada *self, const std::string &topicName, const std::string &baseFrameName) + -> std::shared_ptr { + int argc = 1; + char *argv[] = {"adapy"}; + ros::init(argc, argv, "ada"); + auto viewer = std::make_shared( + self->getWorld(), + topicName, + baseFrameName); + viewer->setAutoUpdate(true); + return viewer; + }); + py::class_>(m, "AdaHand") + .def("get_skeleton", + [](ada::AdaHand *self) -> dart::dynamics::MetaSkeletonPtr { + return self->getMetaSkeleton(); + }) + .def("get_state_space", + [](ada::AdaHand *self) -> aikido::statespace::dart::MetaSkeletonStateSpacePtr { + auto handSkeleton = self->getMetaSkeleton(); + auto handSpace = std::make_shared(handSkeleton.get()); + return handSpace; + }) + .def("execute_preshape", + [](ada::AdaHand *self, const Eigen::Vector2d &d)-> void { + auto future = self->executePreshape(d); +// future.wait(); +// ros::Duration(5).sleep(); + }) + .def("get_endeffector_transform", + [](ada::AdaHand *self, + const std::string& objectType) -> Eigen::Matrix4d { + return self->getEndEffectorTransform(objectType)->matrix(); + }) + .def("get_endeffector_body_node", + [](ada::AdaHand *self) -> dart::dynamics::BodyNode* { + return self->getEndEffectorBodyNode(); + }) + .def("grab", + [](ada::AdaHand *self, dart::dynamics::SkeletonPtr object) -> void { + self->grab(object); + }); +} + diff --git a/src/adapy/adapy.cpp b/src/adapy/adapy.cpp new file mode 100644 index 0000000..9075b31 --- /dev/null +++ b/src/adapy/adapy.cpp @@ -0,0 +1,24 @@ +#include +#include +#include +#include + +namespace py = pybind11; + +void Ada(pybind11::module& m); +void Aikido(pybind11::module& m); +void Dart(pybind11::module& m); +void Pr_Tsr(pybind11::module& m); +void IK(pybind11::module& m); + +PYBIND11_MODULE(adapy, m) { + Ada(m); + + Aikido(m); + + Dart(m); + + Pr_Tsr(m); + + IK(m); +} diff --git a/src/adapy/aikido_py.cpp b/src/adapy/aikido_py.cpp new file mode 100644 index 0000000..cd6a163 --- /dev/null +++ b/src/adapy/aikido_py.cpp @@ -0,0 +1,128 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "aikido/statespace/StateHandle.hpp" +#include "aikido/statespace/StateSpace.hpp" +#include "aikido/statespace/ScopedState.hpp" + +namespace py = pybind11; + + + +Eigen::Isometry3d vectorToIsometry(std::vector &poseVector) { + double *ptr = &poseVector[0]; + Eigen::Map p(ptr, 7); + + Eigen::Isometry3d pose(Eigen::Isometry3d::Identity()); + pose.translation() = p.head(3); + Eigen::Quaterniond q(p[3], p[4], p[5], p[6]); + pose.linear() = Eigen::Matrix3d(q); + return pose; +} + +Eigen::Isometry3d matrixToIsometry(Eigen::Matrix4d& poseMatrix) { + Eigen::Isometry3d pose(Eigen::Isometry3d::Identity()); + pose.translation() = poseMatrix.block<3, 1>(0, 3); + pose.linear() = poseMatrix.block<3, 3>(0, 0); + return pose; +} + + +void Aikido(pybind11::module& m) { + //====================================AIKIDO============================================================================= + py::class_>(m, "World") + .def("add_body_from_urdf", [](aikido::planner::World *self, const std::string &uri, + std::vector objectPose) -> std::shared_ptr<::dart::dynamics::Skeleton> { + auto transform = vectorToIsometry(objectPose); + + dart::utils::DartLoader urdfLoader; + const auto resourceRetriever + = std::make_shared(); + const auto skeleton = urdfLoader.parseSkeleton(uri, resourceRetriever); + + if (!skeleton) + throw std::runtime_error("unable to load '" + uri + "'"); + + dynamic_cast(skeleton->getJoint(0)) + ->setTransform(transform); + + self->addSkeleton(skeleton); + return skeleton; + }) + .def("add_body_from_urdf_matrix", [](aikido::planner::World *self, const std::string& uri, + Eigen::Matrix4d& objectPose) -> std::shared_ptr<::dart::dynamics::Skeleton> { + auto transform = matrixToIsometry(objectPose); + + dart::utils::DartLoader urdfLoader; + const auto resourceRetriever + = std::make_shared(); + const auto skeleton = urdfLoader.parseSkeleton(uri, resourceRetriever); + + if (!skeleton) + throw std::runtime_error("unable to load '" + uri + "'"); + + dynamic_cast(skeleton->getJoint(0)) + ->setTransform(transform); + + self->addSkeleton(skeleton); + return skeleton; + }) + .def("remove_skeleton", [](aikido::planner::World *self, std::shared_ptr<::dart::dynamics::Skeleton> skeleton) -> void { + self->removeSkeleton(skeleton); + }) + .def("get_skeleton", [](aikido::planner::World *self, int i) -> dart::dynamics::SkeletonPtr { + if (i + 1 <= int(self->getNumSkeletons())) { + return self->getSkeleton(i); + } + else { + return nullptr; + } + }); + py::class_>( + m, "WorldInteractiveMarkerViewer") + .def("update", + [](aikido::rviz::WorldInteractiveMarkerViewer *self)->void{self->update();}) + .def("add_frame", + [](aikido::rviz::WorldInteractiveMarkerViewer *self, dart::dynamics::BodyNode* node) + -> void { + self->addFrame(node); + }) + .def("add_tsr_marker", + [](aikido::rviz::WorldInteractiveMarkerViewer *self, + std::shared_ptr tsr) + -> aikido::rviz::TSRMarkerPtr { + return self->addTSRMarker(*tsr.get()); + }); + py::class_>( + m, + "CollisionFree"); + py::class_( + m, + "MetaSkeletonStateSpace"); + py::class_(m, "Trajectory"); + py::class_(m, "TSRMarker"); + py::class_(m, "FullCollisionFree"). + def("is_satisfied", + [](aikido::constraint::Testable *self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& armSpace, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + const Eigen::VectorXd& positions) -> bool { + auto armState = armSpace->createState(); + armSpace->convertPositionsToState(positions, armState); + auto currentState = armSpace->getScopedStateFromMetaSkeleton(armSkeleton.get()); + aikido::constraint::DefaultTestableOutcome fullCollisionCheckOutcome; + bool collisionResult = self->isSatisfied(armState, &fullCollisionCheckOutcome); + armSpace->setState(armSkeleton.get(), currentState); + return collisionResult; + }); +} diff --git a/src/adapy/dart_py.cpp b/src/adapy/dart_py.cpp new file mode 100644 index 0000000..8d2e01f --- /dev/null +++ b/src/adapy/dart_py.cpp @@ -0,0 +1,40 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +void Dart(pybind11::module& m) { + //===================================================DART====================================================================== + py::class_>(m, "Skeleton") + .def("get_name", + [](dart::dynamics::Skeleton *self) -> std::string { return self->getName(); }) + .def("get_positions", + [](dart::dynamics::Skeleton *self) -> Eigen::VectorXd { return self->getPositions(); }) + .def("get_num_joints", + [](dart::dynamics::Skeleton *self) -> int { + return self->getNumJoints(); + }); + + py::class_>(m, "MetaSkeleton") + .def("get_name", + [](dart::dynamics::MetaSkeleton *self) -> std::string { return self->getName(); }) + .def("get_positions", + [](dart::dynamics::MetaSkeleton *self) -> Eigen::VectorXd { return self->getPositions(); }) + .def("get_num_joints", + [](dart::dynamics::MetaSkeleton *self) -> int { + return self->getNumJoints(); + }) + .def("get_linear_jacobian", + [](dart::dynamics::MetaSkeleton *self, const dart::dynamics::BodyNode* _node) ->dart::math::LinearJacobian{ + return self->getLinearJacobian(_node);}) + .def("get_jacobian", + [](dart::dynamics::MetaSkeleton *self, const dart::dynamics::BodyNode* _node) ->dart::math::Jacobian{ + return self->getJacobian(_node);}); +} + diff --git a/src/adapy/ik_py.cpp b/src/adapy/ik_py.cpp new file mode 100644 index 0000000..33c4922 --- /dev/null +++ b/src/adapy/ik_py.cpp @@ -0,0 +1,55 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +void IK(pybind11::module& m) { + m.def("create_ik", []( + dart::dynamics::MetaSkeletonPtr armSkeleton, + aikido::statespace::dart::MetaSkeletonStateSpacePtr armSpace, + aikido::constraint::dart::TSR objectTSR, + dart::dynamics::BodyNode* body_node) -> + aikido::constraint::dart::InverseKinematicsSampleable { + auto goalTSR = std::make_shared(objectTSR); + auto ik = dart::dynamics::InverseKinematics::create(body_node); + ik->setDofs(armSkeleton->getDofs()); + auto rng = std::unique_ptr(new aikido::common::RNGWrapper(0)); + aikido::constraint::dart::InverseKinematicsSampleable ikSampleable(armSpace, + armSkeleton, + goalTSR, + aikido::constraint::dart::createSampleableBounds(armSpace, std::move(rng)), + ik, + 10); + return ikSampleable; + }); + + py::class_>(m, "IKSampleable") + .def("create_sample_generator", [](aikido::constraint::dart::InverseKinematicsSampleable* self) -> std::unique_ptr { + return self->createSampleGenerator(); + }); + + py::class_(m, "IKSampleGenerator") + .def("can_sample", [](aikido::constraint::SampleGenerator *self) -> bool { + return self->canSample(); + }) + .def("sample", [](aikido::constraint::SampleGenerator *self, + aikido::statespace::dart::MetaSkeletonStateSpacePtr armSpace) -> Eigen::VectorXd { + auto goalState = armSpace->createState(); + bool sampled = self->sample(goalState); + Eigen::VectorXd positions; + if (!sampled) return positions; + armSpace->convertStateToPositions(goalState, positions); + return positions; + }); + + py::class_>(m, "BodyNode"); +} + diff --git a/src/adapy/pr_tsr_py.cpp b/src/adapy/pr_tsr_py.cpp new file mode 100644 index 0000000..5a995b1 --- /dev/null +++ b/src/adapy/pr_tsr_py.cpp @@ -0,0 +1,37 @@ +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +Eigen::Isometry3d vectorToIsometry(std::vector &poseVector); +Eigen::Isometry3d matrixToIsometry(Eigen::Matrix4d& poseMatrix); + +void Pr_Tsr(pybind11::module& m) { + m.def("get_default_TSR", &pr_tsr::getDefaultCanTSR); + + py::class_>(m, "TSR") + .def("get_T0_w", [](aikido::constraint::dart::TSR *self) -> Eigen::Matrix4d { + auto Trans = self->mT0_w.matrix(); + return Trans; + }) + .def("set_T0_w", [](aikido::constraint::dart::TSR *self, Eigen::Matrix4d& pose) -> void { + auto transform = matrixToIsometry(pose); + self->mT0_w = transform; + }) + .def("set_Bw", [](aikido::constraint::dart::TSR *self, Eigen::MatrixXf& Bw) -> void { + self->mBw = Bw.cast(); + }) + .def("get_Tw_e", [](aikido::constraint::dart::TSR *self) -> Eigen::Matrix4d { + auto Trans = self->mTw_e.matrix(); + return Trans; + }) + .def("set_Tw_e", [](aikido::constraint::dart::TSR *self, Eigen::Matrix4d& pose) -> void { + auto transform = matrixToIsometry(pose); + self->mTw_e = transform; + }); +} + diff --git a/src/Ada.cpp b/src/libada/Ada.cpp similarity index 100% rename from src/Ada.cpp rename to src/libada/Ada.cpp diff --git a/src/AdaFingerKinematicSimulationPositionCommandExecutor.cpp b/src/libada/AdaFingerKinematicSimulationPositionCommandExecutor.cpp similarity index 100% rename from src/AdaFingerKinematicSimulationPositionCommandExecutor.cpp rename to src/libada/AdaFingerKinematicSimulationPositionCommandExecutor.cpp diff --git a/src/AdaHand.cpp b/src/libada/AdaHand.cpp similarity index 100% rename from src/AdaHand.cpp rename to src/libada/AdaHand.cpp diff --git a/src/AdaHandKinematicSimulationPositionCommandExecutor.cpp b/src/libada/AdaHandKinematicSimulationPositionCommandExecutor.cpp similarity index 100% rename from src/AdaHandKinematicSimulationPositionCommandExecutor.cpp rename to src/libada/AdaHandKinematicSimulationPositionCommandExecutor.cpp diff --git a/src/util.cpp b/src/libada/util.cpp similarity index 100% rename from src/util.cpp rename to src/libada/util.cpp diff --git a/src/scripts/simple_trajectories.py b/src/scripts/simple_trajectories.py new file mode 100755 index 0000000..3b3905f --- /dev/null +++ b/src/scripts/simple_trajectories.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python + +import adapy +import rospy + +import pdb +pdb.set_trace() + +rospy.init_node("adapy_simple_traj") +rate = rospy.Rate(10) + +if not rospy.is_shutdown(): + ada = adapy.Ada(True) + 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] + 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() + + arm_skeleton = ada.get_arm_skeleton() + positions = arm_skeleton.get_positions() + arm_state_space = ada.get_arm_state_space() + + positions2 = positions.copy() + positions3 = positions.copy() + positions4 = positions.copy() + positions2[0] += 0.5 + positions2[2] += 0.2 + positions3[0] += 1.5 + positions3[2] += 0.4 + positions4[1] -= 0.4 + positions4[2] += 0.6 + + waypoints = [(0.0, positions), (1.0, positions2), (2.0, positions3), (3.0, positions4)] + traj = ada.compute_joint_space_path(arm_state_space, waypoints) + ada.execute_trajectory(traj) + + rospy.sleep(1.0) diff --git a/src/scripts/test_collisions.py b/src/scripts/test_collisions.py new file mode 100644 index 0000000..d10351d --- /dev/null +++ b/src/scripts/test_collisions.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python + +import adapy +import rospy + +# import pdb +# pdb.set_trace() + +rospy.init_node("adapy_simple_traj") +rate = rospy.Rate(10) + +ada = adapy.Ada(True) +viewer = ada.start_viewer("dart_markers/simple_trajectories", "map") +canURDFUri = "package://pr_assets/data/objects/can.urdf" +sodaCanPose = [0.5, 0.0, 0.73, 0, 0, 0, 1] +tableURDFUri = "package://pr_assets/data/furniture/uw_demo_table.urdf" +tablePose = [0.5, 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) + +arm_skeleton = ada.get_arm_skeleton() +positions = arm_skeleton.get_positions() +arm_state_space = ada.get_arm_state_space() + + +collision_free_constraint = ada.set_up_collision_detection(arm_state_space, arm_skeleton, [can, table]) +full_collision_free_constraint = ada.get_full_collision_constraint(arm_state_space, arm_skeleton, collision_free_constraint) + +# test original position +safe = full_collision_free_constraint.is_satisfied(arm_state_space, arm_skeleton, positions) +if not safe: + print("State state is not safe!") + +# test self-collision +positions2 = positions.copy() +positions2[1] = 0.4 +safe = full_collision_free_constraint.is_satisfied(arm_state_space, arm_skeleton, positions2) +if not safe: + print("Self-collision!") + +# test robot arm collision +positions3 = positions.copy() +positions3[1] -= 0.4 +positions3[2] += 0.6 +safe = full_collision_free_constraint.is_satisfied(arm_state_space, arm_skeleton, positions3) +if not safe: + print("Arm body is in collision!") + +# test robot hand collision +positions4 = positions.copy() +positions4[1] = 2.1 +safe = full_collision_free_constraint.is_satisfied(arm_state_space, arm_skeleton, positions4) +if not safe: + print("Hand is in collision!") + From 7097a8f890ff5fbbfd3d57976f72b2af271e6cc8 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Wed, 24 Feb 2021 15:35:55 -0800 Subject: [PATCH 02/38] update dependencies in package.xml --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index 84c810f..99627b6 100644 --- a/package.xml +++ b/package.xml @@ -14,6 +14,7 @@ roslib srdfdom urdf + roscpp_initializer nlopt From 51d6fe94f1e252457e7ebe52bcdb8c710a2c04b9 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Thu, 25 Feb 2021 22:50:22 -0800 Subject: [PATCH 03/38] Downgrade 3.8 -> 3.6 python version. --- CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index eb70679..8863328 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ project(libada) set(library_VERSION 0.3.0) if(NOT ADAPY_PYTHON_VERSION) - set(ADAPY_PYTHON_VERSION 3.8) + set(ADAPY_PYTHON_VERSION 3.6) endif() find_package(PythonInterp ${ADAPY_PYTHON_VERSION} REQUIRED) @@ -55,7 +55,7 @@ find_package(aikido 0.3.0 REQUIRED io ) -find_package(PythonLibs 3.8 REQUIRED) +find_package(PythonLibs 3.6 REQUIRED) find_package(ada_description REQUIRED) @@ -131,7 +131,7 @@ target_link_libraries(libada ) #============================================================================= -#Pybind +#Pybind pybind11_add_module(adapy MODULE @@ -187,7 +187,7 @@ install(TARGETS libada EXPORT libadaConfig install(DIRECTORY include/ DESTINATION include) install(TARGETS adapy - LIBRARY DESTINATION lib/python3.8/dist-packages) + LIBRARY DESTINATION lib/python3.6/dist-packages) # This makes the project importable from the install directory install(EXPORT libadaConfig DESTINATION share/libada/cmake) From b40932c18296a1012b547bc54ff7e3609945803c Mon Sep 17 00:00:00 2001 From: sniyaz Date: Thu, 25 Feb 2021 22:55:37 -0800 Subject: [PATCH 04/38] Fix rviz search in CMake. --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8863328..b3d1d18 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -33,8 +33,7 @@ endif() include(ExternalProject) find_package(DART 6.8.5 REQUIRED - COMPONENTS utils utils-urdf optimizer-nlopt rviz - + COMPONENTS utils utils-urdf optimizer-nlopt ) find_package(aikido 0.3.0 REQUIRED @@ -53,6 +52,7 @@ find_package(aikido 0.3.0 REQUIRED robot statespace io + rviz ) find_package(PythonLibs 3.6 REQUIRED) From 5bbeeedf322c3e1d549ebc33d1e20c977d2884de Mon Sep 17 00:00:00 2001 From: sniyaz Date: Fri, 26 Feb 2021 03:21:44 -0800 Subject: [PATCH 05/38] Think we've found pybind11 the right way? --- CMakeLists.txt | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index b3d1d18..de294fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,6 +7,24 @@ if(NOT ADAPY_PYTHON_VERSION) endif() find_package(PythonInterp ${ADAPY_PYTHON_VERSION} REQUIRED) +execute_process(COMMAND ${PYTHON_EXECUTABLE} -c + "from distutils.sysconfig import get_python_lib;\ + print(get_python_lib(plat_specific=True, prefix=''))" + OUTPUT_VARIABLE PYTHON_SITE_PACKAGES + OUTPUT_STRIP_TRAILING_WHITESPACE +) +if(NOT PythonInterp_FOUND) + message(WARNING "Failed to find PythonInterp.") + return() +endif() + +find_package(PythonLibs ${ADAPY_PYTHON_VERSION} REQUIRED) + +# Find pybind11 +# Needs to set PYBIND11_PYTHON_VERSION before finding pybind11 +set(PYBIND11_PYTHON_VERSION ${ADAPY_PYTHON_VERSION}) +find_package(pybind11 2.0 REQUIRED) + option(LIBADA_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF) From 7002618817d494ae40000882ccef038e010fa035 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Feb 2021 01:08:57 -0800 Subject: [PATCH 06/38] Manage to fight to some odd bug in ada_py.cpp. --- CMakeLists.txt | 14 ++++++++------ src/adapy/ada_py.cpp | 3 +-- src/adapy/adapy.cpp | 6 +++++- src/adapy/aikido_py.cpp | 14 +++++++------- 4 files changed, 21 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index de294fc..1549c56 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -105,12 +105,6 @@ set(sources src/libada/AdaHandKinematicSimulationPositionCommandExecutor.cpp src/libada/AdaHand.cpp src/libada/Ada.cpp - src/adapy/adapy.cpp - src/adapy/ada_py.cpp - src/adapy/aikido_py.cpp - src/adapy/dart_py.cpp - src/adapy/pr_tsr_py.cpp - src/adapy/ik_py.cpp src/libada/util.cpp ) @@ -161,6 +155,14 @@ pybind11_add_module(adapy src/adapy/ik_py.cpp ) +target_include_directories(adapy + SYSTEM PUBLIC + ${PYTHON_INCLUDE_DIRS} + ${pybind11_INCLUDE_DIRS} + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} +) + target_link_libraries(adapy PUBLIC libada ${libaikidopy_LIBRARIES} diff --git a/src/adapy/ada_py.cpp b/src/adapy/ada_py.cpp index 5a3348a..f93f156 100644 --- a/src/adapy/ada_py.cpp +++ b/src/adapy/ada_py.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -170,4 +170,3 @@ void Ada(pybind11::module& m) { self->grab(object); }); } - diff --git a/src/adapy/adapy.cpp b/src/adapy/adapy.cpp index 9075b31..8064c10 100644 --- a/src/adapy/adapy.cpp +++ b/src/adapy/adapy.cpp @@ -11,7 +11,9 @@ void Dart(pybind11::module& m); void Pr_Tsr(pybind11::module& m); void IK(pybind11::module& m); -PYBIND11_MODULE(adapy, m) { +PYBIND11_PLUGIN(adapy) { + pybind11::module m("adapy"); + Ada(m); Aikido(m); @@ -21,4 +23,6 @@ PYBIND11_MODULE(adapy, m) { Pr_Tsr(m); IK(m); + + return m.ptr(); } diff --git a/src/adapy/aikido_py.cpp b/src/adapy/aikido_py.cpp index cd6a163..4a46b01 100644 --- a/src/adapy/aikido_py.cpp +++ b/src/adapy/aikido_py.cpp @@ -1,7 +1,7 @@ #include #include #include -#include +#include #include #include #include @@ -88,17 +88,17 @@ void Aikido(pybind11::module& m) { return nullptr; } }); - py::class_>( - m, "WorldInteractiveMarkerViewer") + py::class_>( + m, "InteractiveMarkerViewer") .def("update", - [](aikido::rviz::WorldInteractiveMarkerViewer *self)->void{self->update();}) + [](aikido::rviz::InteractiveMarkerViewer *self)->void{self->update();}) .def("add_frame", - [](aikido::rviz::WorldInteractiveMarkerViewer *self, dart::dynamics::BodyNode* node) + [](aikido::rviz::InteractiveMarkerViewer *self, dart::dynamics::BodyNode* node) -> void { - self->addFrame(node); + self->addFrameMarker(node); }) .def("add_tsr_marker", - [](aikido::rviz::WorldInteractiveMarkerViewer *self, + [](aikido::rviz::InteractiveMarkerViewer *self, std::shared_ptr tsr) -> aikido::rviz::TSRMarkerPtr { return self->addTSRMarker(*tsr.get()); From fe4943618a4b74df3969f76d7f1c06f02a8d7c4e Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Feb 2021 02:16:41 -0800 Subject: [PATCH 07/38] Revert to pbind11 2.2. --- CMakeLists.txt | 2 +- src/adapy/adapy.cpp | 6 +----- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1549c56..f2bb381 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,7 +23,7 @@ find_package(PythonLibs ${ADAPY_PYTHON_VERSION} REQUIRED) # Find pybind11 # Needs to set PYBIND11_PYTHON_VERSION before finding pybind11 set(PYBIND11_PYTHON_VERSION ${ADAPY_PYTHON_VERSION}) -find_package(pybind11 2.0 REQUIRED) +find_package(pybind11 2.2.0 REQUIRED) option(LIBADA_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF) diff --git a/src/adapy/adapy.cpp b/src/adapy/adapy.cpp index 8064c10..9075b31 100644 --- a/src/adapy/adapy.cpp +++ b/src/adapy/adapy.cpp @@ -11,9 +11,7 @@ void Dart(pybind11::module& m); void Pr_Tsr(pybind11::module& m); void IK(pybind11::module& m); -PYBIND11_PLUGIN(adapy) { - pybind11::module m("adapy"); - +PYBIND11_MODULE(adapy, m) { Ada(m); Aikido(m); @@ -23,6 +21,4 @@ PYBIND11_PLUGIN(adapy) { Pr_Tsr(m); IK(m); - - return m.ptr(); } From bbbcb2c63ca8f09a6c646b094c10f4e22e0667f7 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Feb 2021 03:16:43 -0800 Subject: [PATCH 08/38] Add two ICAROS methods for computing joint-space paths, but I don't like them. --- include/libada/Ada.hpp | 9 +++++++ src/libada/Ada.cpp | 54 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 63 insertions(+) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 902925b..3c78eec 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -176,6 +176,15 @@ class Ada final : public aikido::robot::Robot // Runs step with current time. void update(); + aikido::trajectory::TrajectoryPtr computeJointSpacePath( + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, + const std::vector>& waypoints); + + aikido::trajectory::TrajectoryPtr computeSmoothJointSpacePath( + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, + const std::vector>& waypoints, + const aikido::constraint::dart::CollisionFreePtr& collisionFree = nullptr); + /// \copydoc Robot::planToConfiguration. aikido::trajectory::TrajectoryPtr planToConfiguration( const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, diff --git a/src/libada/Ada.cpp b/src/libada/Ada.cpp index 30a5e4b..749fb03 100644 --- a/src/libada/Ada.cpp +++ b/src/libada/Ada.cpp @@ -9,12 +9,14 @@ #include #include +#include #include #include #include #include #include #include +#include #include #include #include @@ -40,6 +42,7 @@ using aikido::constraint::dart::TSR; using aikido::constraint::dart::TSRPtr; using aikido::control::TrajectoryExecutorPtr; using aikido::planner::kunzretimer::KunzRetimer; +using aikido::planner::parabolic::ParabolicSmoother; using aikido::robot::ConcreteManipulator; using aikido::robot::ConcreteManipulatorPtr; using aikido::robot::ConcreteRobot; @@ -362,6 +365,57 @@ void Ada::update() step(std::chrono::system_clock::now()); } +//============================================================================== +TrajectoryPtr Ada::computeJointSpacePath( + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, + const std::vector>& waypoints) { + auto satisfied = std::make_shared(stateSpace); + + std::shared_ptr + interpolator = std::make_shared(stateSpace); + std::shared_ptr + traj = std::make_shared(stateSpace, interpolator); + + for (auto &waypoint : waypoints) { + auto state = stateSpace->createState(); + stateSpace->convertPositionsToState(waypoint.second, state); + traj->addWaypoint(waypoint.first, state); + } + + auto timedTrajectory + = std::move(postProcessPath( + traj.get(), satisfied, ada::KunzParams())); + + return std::move(timedTrajectory); +} + +//============================================================================== +TrajectoryPtr Ada::computeSmoothJointSpacePath( + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, + const std::vector>& waypoints, + const CollisionFreePtr& collisionFree) { + // Use the given constraint if one was given. + TestablePtr constraint = collisionFree; + if (!constraint) + constraint = std::make_shared(stateSpace); + + std::shared_ptr + interpolator = std::make_shared(stateSpace); + std::shared_ptr + traj = std::make_shared(stateSpace, interpolator); + + for (auto &waypoint : waypoints) { + auto state = stateSpace->createState(); + stateSpace->convertPositionsToState(waypoint.second, state); + traj->addWaypoint(waypoint.first, state); + } + + auto smoothTrajectory + = postProcessPath(traj.get(), constraint, ParabolicSmoother::Params()); + + return std::move(smoothTrajectory); +} + //============================================================================== TrajectoryPtr Ada::planToConfiguration( const MetaSkeletonStateSpacePtr& space, From 9dbad3f8064f9c1b1a773e612e84fecc7e1dd4d1 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Feb 2021 03:18:31 -0800 Subject: [PATCH 09/38] Run make format for my OCD. --- include/libada/Ada.hpp | 3 +- src/adapy/ada_py.cpp | 349 ++++++++++++++++++++++------------------ src/adapy/adapy.cpp | 7 +- src/adapy/aikido_py.cpp | 218 ++++++++++++++----------- src/adapy/dart_py.cpp | 83 ++++++---- src/adapy/ik_py.cpp | 103 +++++++----- src/adapy/pr_tsr_py.cpp | 66 +++++--- src/libada/Ada.cpp | 41 ++--- 8 files changed, 505 insertions(+), 365 deletions(-) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 3c78eec..1d76ed5 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -183,7 +183,8 @@ class Ada final : public aikido::robot::Robot aikido::trajectory::TrajectoryPtr computeSmoothJointSpacePath( const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, const std::vector>& waypoints, - const aikido::constraint::dart::CollisionFreePtr& collisionFree = nullptr); + const aikido::constraint::dart::CollisionFreePtr& collisionFree + = nullptr); /// \copydoc Robot::planToConfiguration. aikido::trajectory::TrajectoryPtr planToConfiguration( diff --git a/src/adapy/ada_py.cpp b/src/adapy/ada_py.cpp index f93f156..87b83bc 100644 --- a/src/adapy/ada_py.cpp +++ b/src/adapy/ada_py.cpp @@ -1,172 +1,213 @@ #include #include -#include -#include #include +#include +#include #include +#include #include "libada/Ada.hpp" #include "aikido/statespace/ScopedState.hpp" -#include namespace py = pybind11; -void Ada(pybind11::module& m) { +void Ada(pybind11::module& m) +{ //====================================LIBADA========================================================== py::class_>(m, "Ada") .def(py::init([](bool simulation) -> std::shared_ptr { return std::make_shared( aikido::planner::World::create(), simulation); })) - .def("get_self_collision_constraint", - [](ada::Ada *self) -> std::shared_ptr { - return self->getSelfCollisionConstraint( - self->getStateSpace(), - self->getMetaSkeleton() - ); - }) - .def("start_trajectory_executor", [](ada::Ada *self) -> void { - self->startTrajectoryExecutor(); - }) - .def("get_name", - [](ada::Ada *self) -> std::string { return self->getName(); }) - .def("get_world", - [](ada::Ada *self) -> aikido::planner::WorldPtr { - return self->getWorld(); - }) - .def("get_hand", - [](ada::Ada *self) -> ada::AdaHandPtr { - return self->getHand(); - }) - .def("get_skeleton", [](ada::Ada *self) -> dart::dynamics::MetaSkeletonPtr { - return self->getMetaSkeleton(); - }) - .def("get_arm_skeleton", [](ada::Ada *self) -> dart::dynamics::MetaSkeletonPtr { - return self->getArm()->getMetaSkeleton(); - }) - .def("get_arm_state_space", [](ada::Ada *self) -> aikido::statespace::dart::MetaSkeletonStateSpacePtr { - return self->getArm()->getStateSpace(); - }) - .def("set_positions",[](ada::Ada *self,const Eigen::VectorXd& configuration) -> void { - auto arm = self->getArm(); - auto armSkeleton = arm->getMetaSkeleton(); - armSkeleton->setPositions(configuration); - }) - .def("set_up_collision_detection", - [](ada::Ada *self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr &armSpace, - const dart::dynamics::MetaSkeletonPtr& armSkeleton, - const std::vector& envSkeletons) -> std::shared_ptr { - dart::collision::FCLCollisionDetectorPtr collisionDetector = dart::collision::FCLCollisionDetector::create(); - std::shared_ptr armCollisionGroup = - collisionDetector->createCollisionGroup(self->getMetaSkeleton().get()); - std::shared_ptr envCollisionGroup = - collisionDetector->createCollisionGroup(); - for (const auto& envSkeleton : envSkeletons) { - envCollisionGroup->addShapeFramesOf(envSkeleton.get()); - } - std::shared_ptr collisionFreeConstraint = - std::make_shared(armSpace, - armSkeleton, - collisionDetector); - collisionFreeConstraint->addPairwiseCheck(armCollisionGroup, envCollisionGroup); - return collisionFreeConstraint; - }) - .def("get_full_collision_constraint", - [](ada::Ada *self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr &armSpace, - const dart::dynamics::MetaSkeletonPtr& armSkeleton, - const aikido::constraint::dart::CollisionFreePtr& collisionFreeConstraint) -> aikido::constraint::TestablePtr { - return self->getFullCollisionConstraint(armSpace, armSkeleton, collisionFreeConstraint); - }) - .def("compute_joint_space_path", - [](ada::Ada *self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr &stateSpace, - const std::vector> &waypoints) - -> aikido::trajectory::TrajectoryPtr { - return self->computeJointSpacePath(stateSpace, waypoints); - }) - .def("compute_smooth_joint_space_path", - [](ada::Ada *self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr &stateSpace, - const std::vector> &waypoints, - const aikido::constraint::dart::CollisionFreePtr &collisionFreeConstraint=nullptr) - -> aikido::trajectory::TrajectoryPtr { - return self->computeSmoothJointSpacePath(stateSpace, waypoints, collisionFreeConstraint); - }) - .def("compute_retime_path", - [](ada::Ada *self, - const dart::dynamics::MetaSkeletonPtr& armSkeleton, - aikido::trajectory::TrajectoryPtr trajectory_ptr) -> aikido::trajectory::TrajectoryPtr { - return self->retimePath(armSkeleton, trajectory_ptr.get()); - }) - .def("plan_to_configuration", - [](ada::Ada *self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr &armSpace, - const dart::dynamics::MetaSkeletonPtr& armSkeleton, - const Eigen::VectorXd& configuration) -> aikido::trajectory::TrajectoryPtr { - auto state = armSpace->createState(); - armSpace->convertPositionsToState(configuration, state); - auto trajectory = self->planToConfiguration(armSpace, - armSkeleton, - state, - nullptr, - 10); - return trajectory; - }) - .def("execute_trajectory", - [](ada::Ada *self, - const aikido::trajectory::TrajectoryPtr &trajectory) - -> void { - auto future = self->executeTrajectory(trajectory); + .def( + "get_self_collision_constraint", + [](ada::Ada* self) + -> std::shared_ptr { + return self->getSelfCollisionConstraint( + self->getStateSpace(), self->getMetaSkeleton()); + }) + .def( + "start_trajectory_executor", + [](ada::Ada* self) -> void { self->startTrajectoryExecutor(); }) + .def( + "get_name", + [](ada::Ada* self) -> std::string { return self->getName(); }) + .def( + "get_world", + [](ada::Ada* self) -> aikido::planner::WorldPtr { + return self->getWorld(); + }) + .def( + "get_hand", + [](ada::Ada* self) -> ada::AdaHandPtr { return self->getHand(); }) + .def( + "get_skeleton", + [](ada::Ada* self) -> dart::dynamics::MetaSkeletonPtr { + return self->getMetaSkeleton(); + }) + .def( + "get_arm_skeleton", + [](ada::Ada* self) -> dart::dynamics::MetaSkeletonPtr { + return self->getArm()->getMetaSkeleton(); + }) + .def( + "get_arm_state_space", + [](ada::Ada* self) + -> aikido::statespace::dart::MetaSkeletonStateSpacePtr { + return self->getArm()->getStateSpace(); + }) + .def( + "set_positions", + [](ada::Ada* self, const Eigen::VectorXd& configuration) -> void { + auto arm = self->getArm(); + auto armSkeleton = arm->getMetaSkeleton(); + armSkeleton->setPositions(configuration); + }) + .def( + "set_up_collision_detection", + [](ada::Ada* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& + armSpace, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + const std::vector& envSkeletons) + -> std::shared_ptr { + dart::collision::FCLCollisionDetectorPtr collisionDetector + = dart::collision::FCLCollisionDetector::create(); + std::shared_ptr armCollisionGroup + = collisionDetector->createCollisionGroup( + self->getMetaSkeleton().get()); + std::shared_ptr envCollisionGroup + = collisionDetector->createCollisionGroup(); + for (const auto& envSkeleton : envSkeletons) + { + envCollisionGroup->addShapeFramesOf(envSkeleton.get()); + } + std::shared_ptr + collisionFreeConstraint + = std::make_shared( + armSpace, armSkeleton, collisionDetector); + collisionFreeConstraint->addPairwiseCheck( + armCollisionGroup, envCollisionGroup); + return collisionFreeConstraint; + }) + .def( + "get_full_collision_constraint", + [](ada::Ada* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& + armSpace, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + const aikido::constraint::dart::CollisionFreePtr& + collisionFreeConstraint) -> aikido::constraint::TestablePtr { + return self->getFullCollisionConstraint( + armSpace, armSkeleton, collisionFreeConstraint); + }) + .def( + "compute_joint_space_path", + [](ada::Ada* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& + stateSpace, + const std::vector>& waypoints) + -> aikido::trajectory::TrajectoryPtr { + return self->computeJointSpacePath(stateSpace, waypoints); + }) + .def( + "compute_smooth_joint_space_path", + [](ada::Ada* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& + stateSpace, + const std::vector>& waypoints, + const aikido::constraint::dart::CollisionFreePtr& + collisionFreeConstraint + = nullptr) -> aikido::trajectory::TrajectoryPtr { + return self->computeSmoothJointSpacePath( + stateSpace, waypoints, collisionFreeConstraint); + }) + .def( + "compute_retime_path", + [](ada::Ada* self, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + aikido::trajectory::TrajectoryPtr trajectory_ptr) + -> aikido::trajectory::TrajectoryPtr { + return self->retimePath(armSkeleton, trajectory_ptr.get()); + }) + .def( + "plan_to_configuration", + [](ada::Ada* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& + armSpace, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + const Eigen::VectorXd& configuration) + -> aikido::trajectory::TrajectoryPtr { + auto state = armSpace->createState(); + armSpace->convertPositionsToState(configuration, state); + auto trajectory = self->planToConfiguration( + armSpace, armSkeleton, state, nullptr, 10); + return trajectory; + }) + .def( + "execute_trajectory", + [](ada::Ada* self, + const aikido::trajectory::TrajectoryPtr& trajectory) -> void { + auto future = self->executeTrajectory(trajectory); -// if (!future.valid()) { -// std::__throw_future_error(0); -// } -// -// future.wait(); -// ros::Duration(15).sleep(); - }) - .def("start_viewer", - [](ada::Ada *self, const std::string &topicName, const std::string &baseFrameName) - -> std::shared_ptr { - int argc = 1; - char *argv[] = {"adapy"}; - ros::init(argc, argv, "ada"); - auto viewer = std::make_shared( - self->getWorld(), - topicName, - baseFrameName); - viewer->setAutoUpdate(true); - return viewer; - }); + // if (!future.valid()) { + // std::__throw_future_error(0); + // } + // + // future.wait(); + // ros::Duration(15).sleep(); + }) + .def( + "start_viewer", + [](ada::Ada* self, + const std::string& topicName, + const std::string& baseFrameName) + -> std::shared_ptr { + int argc = 1; + char* argv[] = {"adapy"}; + ros::init(argc, argv, "ada"); + auto viewer + = std::make_shared( + self->getWorld(), topicName, baseFrameName); + viewer->setAutoUpdate(true); + return viewer; + }); py::class_>(m, "AdaHand") - .def("get_skeleton", - [](ada::AdaHand *self) -> dart::dynamics::MetaSkeletonPtr { - return self->getMetaSkeleton(); - }) - .def("get_state_space", - [](ada::AdaHand *self) -> aikido::statespace::dart::MetaSkeletonStateSpacePtr { - auto handSkeleton = self->getMetaSkeleton(); - auto handSpace = std::make_shared(handSkeleton.get()); - return handSpace; - }) - .def("execute_preshape", - [](ada::AdaHand *self, const Eigen::Vector2d &d)-> void { - auto future = self->executePreshape(d); -// future.wait(); -// ros::Duration(5).sleep(); - }) - .def("get_endeffector_transform", - [](ada::AdaHand *self, - const std::string& objectType) -> Eigen::Matrix4d { - return self->getEndEffectorTransform(objectType)->matrix(); - }) - .def("get_endeffector_body_node", - [](ada::AdaHand *self) -> dart::dynamics::BodyNode* { - return self->getEndEffectorBodyNode(); - }) - .def("grab", - [](ada::AdaHand *self, dart::dynamics::SkeletonPtr object) -> void { - self->grab(object); - }); + .def( + "get_skeleton", + [](ada::AdaHand* self) -> dart::dynamics::MetaSkeletonPtr { + return self->getMetaSkeleton(); + }) + .def( + "get_state_space", + [](ada::AdaHand* self) + -> aikido::statespace::dart::MetaSkeletonStateSpacePtr { + auto handSkeleton = self->getMetaSkeleton(); + auto handSpace = std::make_shared< + aikido::statespace::dart::MetaSkeletonStateSpace>( + handSkeleton.get()); + return handSpace; + }) + .def( + "execute_preshape", + [](ada::AdaHand* self, const Eigen::Vector2d& d) -> void { + auto future = self->executePreshape(d); + // future.wait(); + // ros::Duration(5).sleep(); + }) + .def( + "get_endeffector_transform", + [](ada::AdaHand* self, + const std::string& objectType) -> Eigen::Matrix4d { + return self->getEndEffectorTransform(objectType)->matrix(); + }) + .def( + "get_endeffector_body_node", + [](ada::AdaHand* self) -> dart::dynamics::BodyNode* { + return self->getEndEffectorBodyNode(); + }) + .def( + "grab", + [](ada::AdaHand* self, dart::dynamics::SkeletonPtr object) -> void { + self->grab(object); + }); } diff --git a/src/adapy/adapy.cpp b/src/adapy/adapy.cpp index 9075b31..e2ba7c0 100644 --- a/src/adapy/adapy.cpp +++ b/src/adapy/adapy.cpp @@ -1,6 +1,6 @@ -#include -#include #include +#include +#include #include namespace py = pybind11; @@ -11,7 +11,8 @@ void Dart(pybind11::module& m); void Pr_Tsr(pybind11::module& m); void IK(pybind11::module& m); -PYBIND11_MODULE(adapy, m) { +PYBIND11_MODULE(adapy, m) +{ Ada(m); Aikido(m); diff --git a/src/adapy/aikido_py.cpp b/src/adapy/aikido_py.cpp index 4a46b01..4eb926f 100644 --- a/src/adapy/aikido_py.cpp +++ b/src/adapy/aikido_py.cpp @@ -1,26 +1,25 @@ -#include #include #include -#include #include -#include -#include +#include +#include #include #include -#include -#include +#include +#include #include +#include +#include #include +#include "aikido/statespace/ScopedState.hpp" #include "aikido/statespace/StateHandle.hpp" #include "aikido/statespace/StateSpace.hpp" -#include "aikido/statespace/ScopedState.hpp" namespace py = pybind11; - - -Eigen::Isometry3d vectorToIsometry(std::vector &poseVector) { - double *ptr = &poseVector[0]; +Eigen::Isometry3d vectorToIsometry(std::vector& poseVector) +{ + double* ptr = &poseVector[0]; Eigen::Map p(ptr, 7); Eigen::Isometry3d pose(Eigen::Isometry3d::Identity()); @@ -30,98 +29,135 @@ Eigen::Isometry3d vectorToIsometry(std::vector &poseVector) { return pose; } -Eigen::Isometry3d matrixToIsometry(Eigen::Matrix4d& poseMatrix) { +Eigen::Isometry3d matrixToIsometry(Eigen::Matrix4d& poseMatrix) +{ Eigen::Isometry3d pose(Eigen::Isometry3d::Identity()); pose.translation() = poseMatrix.block<3, 1>(0, 3); pose.linear() = poseMatrix.block<3, 3>(0, 0); return pose; } - -void Aikido(pybind11::module& m) { +void Aikido(pybind11::module& m) +{ //====================================AIKIDO============================================================================= - py::class_>(m, "World") - .def("add_body_from_urdf", [](aikido::planner::World *self, const std::string &uri, - std::vector objectPose) -> std::shared_ptr<::dart::dynamics::Skeleton> { - auto transform = vectorToIsometry(objectPose); - - dart::utils::DartLoader urdfLoader; - const auto resourceRetriever - = std::make_shared(); - const auto skeleton = urdfLoader.parseSkeleton(uri, resourceRetriever); - - if (!skeleton) - throw std::runtime_error("unable to load '" + uri + "'"); - - dynamic_cast(skeleton->getJoint(0)) - ->setTransform(transform); - - self->addSkeleton(skeleton); - return skeleton; - }) - .def("add_body_from_urdf_matrix", [](aikido::planner::World *self, const std::string& uri, - Eigen::Matrix4d& objectPose) -> std::shared_ptr<::dart::dynamics::Skeleton> { - auto transform = matrixToIsometry(objectPose); - - dart::utils::DartLoader urdfLoader; - const auto resourceRetriever - = std::make_shared(); - const auto skeleton = urdfLoader.parseSkeleton(uri, resourceRetriever); - - if (!skeleton) - throw std::runtime_error("unable to load '" + uri + "'"); - - dynamic_cast(skeleton->getJoint(0)) - ->setTransform(transform); - - self->addSkeleton(skeleton); - return skeleton; - }) - .def("remove_skeleton", [](aikido::planner::World *self, std::shared_ptr<::dart::dynamics::Skeleton> skeleton) -> void { - self->removeSkeleton(skeleton); - }) - .def("get_skeleton", [](aikido::planner::World *self, int i) -> dart::dynamics::SkeletonPtr { - if (i + 1 <= int(self->getNumSkeletons())) { - return self->getSkeleton(i); - } - else { - return nullptr; - } - }); - py::class_>( + py::class_>( + m, "World") + .def( + "add_body_from_urdf", + [](aikido::planner::World* self, + const std::string& uri, + std::vector objectPose) + -> std::shared_ptr<::dart::dynamics::Skeleton> { + auto transform = vectorToIsometry(objectPose); + + dart::utils::DartLoader urdfLoader; + const auto resourceRetriever + = std::make_shared(); + const auto skeleton + = urdfLoader.parseSkeleton(uri, resourceRetriever); + + if (!skeleton) + throw std::runtime_error("unable to load '" + uri + "'"); + + dynamic_cast(skeleton->getJoint(0)) + ->setTransform(transform); + + self->addSkeleton(skeleton); + return skeleton; + }) + .def( + "add_body_from_urdf_matrix", + [](aikido::planner::World* self, + const std::string& uri, + Eigen::Matrix4d& objectPose) + -> std::shared_ptr<::dart::dynamics::Skeleton> { + auto transform = matrixToIsometry(objectPose); + + dart::utils::DartLoader urdfLoader; + const auto resourceRetriever + = std::make_shared(); + const auto skeleton + = urdfLoader.parseSkeleton(uri, resourceRetriever); + + if (!skeleton) + throw std::runtime_error("unable to load '" + uri + "'"); + + dynamic_cast(skeleton->getJoint(0)) + ->setTransform(transform); + + self->addSkeleton(skeleton); + return skeleton; + }) + .def( + "remove_skeleton", + [](aikido::planner::World* self, + std::shared_ptr<::dart::dynamics::Skeleton> skeleton) -> void { + self->removeSkeleton(skeleton); + }) + .def( + "get_skeleton", + [](aikido::planner::World* self, + int i) -> dart::dynamics::SkeletonPtr { + if (i + 1 <= int(self->getNumSkeletons())) + { + return self->getSkeleton(i); + } + else + { + return nullptr; + } + }); + py::class_< + aikido::rviz::InteractiveMarkerViewer, + std::shared_ptr>( m, "InteractiveMarkerViewer") - .def("update", - [](aikido::rviz::InteractiveMarkerViewer *self)->void{self->update();}) - .def("add_frame", - [](aikido::rviz::InteractiveMarkerViewer *self, dart::dynamics::BodyNode* node) - -> void { - self->addFrameMarker(node); - }) - .def("add_tsr_marker", - [](aikido::rviz::InteractiveMarkerViewer *self, - std::shared_ptr tsr) - -> aikido::rviz::TSRMarkerPtr { - return self->addTSRMarker(*tsr.get()); - }); - py::class_>( - m, - "CollisionFree"); - py::class_( - m, - "MetaSkeletonStateSpace"); - py::class_(m, "Trajectory"); - py::class_(m, "TSRMarker"); - py::class_(m, "FullCollisionFree"). - def("is_satisfied", - [](aikido::constraint::Testable *self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& armSpace, + .def( + "update", + [](aikido::rviz::InteractiveMarkerViewer* self) -> void { + self->update(); + }) + .def( + "add_frame", + [](aikido::rviz::InteractiveMarkerViewer* self, + dart::dynamics::BodyNode* node) -> void { + self->addFrameMarker(node); + }) + .def( + "add_tsr_marker", + [](aikido::rviz::InteractiveMarkerViewer* self, + std::shared_ptr tsr) + -> aikido::rviz::TSRMarkerPtr { + return self->addTSRMarker(*tsr.get()); + }); + py::class_< + aikido::constraint::dart::CollisionFree, + std::shared_ptr>( + m, "CollisionFree"); + py::class_< + aikido::statespace::dart::MetaSkeletonStateSpace, + aikido::statespace::dart::MetaSkeletonStateSpacePtr>( + m, "MetaSkeletonStateSpace"); + py::class_( + m, "Trajectory"); + py::class_( + m, "TSRMarker"); + py::class_( + m, "FullCollisionFree") + .def( + "is_satisfied", + [](aikido::constraint::Testable* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& + armSpace, const dart::dynamics::MetaSkeletonPtr& armSkeleton, const Eigen::VectorXd& positions) -> bool { auto armState = armSpace->createState(); armSpace->convertPositionsToState(positions, armState); - auto currentState = armSpace->getScopedStateFromMetaSkeleton(armSkeleton.get()); - aikido::constraint::DefaultTestableOutcome fullCollisionCheckOutcome; - bool collisionResult = self->isSatisfied(armState, &fullCollisionCheckOutcome); + auto currentState + = armSpace->getScopedStateFromMetaSkeleton(armSkeleton.get()); + aikido::constraint::DefaultTestableOutcome + fullCollisionCheckOutcome; + bool collisionResult + = self->isSatisfied(armState, &fullCollisionCheckOutcome); armSpace->setState(armSkeleton.get(), currentState); return collisionResult; }); diff --git a/src/adapy/dart_py.cpp b/src/adapy/dart_py.cpp index 8d2e01f..8359ef5 100644 --- a/src/adapy/dart_py.cpp +++ b/src/adapy/dart_py.cpp @@ -1,40 +1,63 @@ -#include -#include #include #include -#include -#include +#include +#include #include +#include +#include #include namespace py = pybind11; -void Dart(pybind11::module& m) { +void Dart(pybind11::module& m) +{ //===================================================DART====================================================================== - py::class_>(m, "Skeleton") - .def("get_name", - [](dart::dynamics::Skeleton *self) -> std::string { return self->getName(); }) - .def("get_positions", - [](dart::dynamics::Skeleton *self) -> Eigen::VectorXd { return self->getPositions(); }) - .def("get_num_joints", - [](dart::dynamics::Skeleton *self) -> int { - return self->getNumJoints(); - }); + py::class_< + dart::dynamics::Skeleton, + std::shared_ptr>(m, "Skeleton") + .def( + "get_name", + [](dart::dynamics::Skeleton* self) -> std::string { + return self->getName(); + }) + .def( + "get_positions", + [](dart::dynamics::Skeleton* self) -> Eigen::VectorXd { + return self->getPositions(); + }) + .def("get_num_joints", [](dart::dynamics::Skeleton* self) -> int { + return self->getNumJoints(); + }); - py::class_>(m, "MetaSkeleton") - .def("get_name", - [](dart::dynamics::MetaSkeleton *self) -> std::string { return self->getName(); }) - .def("get_positions", - [](dart::dynamics::MetaSkeleton *self) -> Eigen::VectorXd { return self->getPositions(); }) - .def("get_num_joints", - [](dart::dynamics::MetaSkeleton *self) -> int { - return self->getNumJoints(); - }) - .def("get_linear_jacobian", - [](dart::dynamics::MetaSkeleton *self, const dart::dynamics::BodyNode* _node) ->dart::math::LinearJacobian{ - return self->getLinearJacobian(_node);}) - .def("get_jacobian", - [](dart::dynamics::MetaSkeleton *self, const dart::dynamics::BodyNode* _node) ->dart::math::Jacobian{ - return self->getJacobian(_node);}); + py::class_< + dart::dynamics::MetaSkeleton, + std::shared_ptr>(m, "MetaSkeleton") + .def( + "get_name", + [](dart::dynamics::MetaSkeleton* self) -> std::string { + return self->getName(); + }) + .def( + "get_positions", + [](dart::dynamics::MetaSkeleton* self) -> Eigen::VectorXd { + return self->getPositions(); + }) + .def( + "get_num_joints", + [](dart::dynamics::MetaSkeleton* self) -> int { + return self->getNumJoints(); + }) + .def( + "get_linear_jacobian", + [](dart::dynamics::MetaSkeleton* self, + const dart::dynamics::BodyNode* _node) + -> dart::math::LinearJacobian { + return self->getLinearJacobian(_node); + }) + .def( + "get_jacobian", + [](dart::dynamics::MetaSkeleton* self, + const dart::dynamics::BodyNode* _node) -> dart::math::Jacobian { + return self->getJacobian(_node); + }); } - diff --git a/src/adapy/ik_py.cpp b/src/adapy/ik_py.cpp index 33c4922..9ba839d 100644 --- a/src/adapy/ik_py.cpp +++ b/src/adapy/ik_py.cpp @@ -1,55 +1,74 @@ -#include +#include #include -#include #include -#include +#include #include -#include -#include +#include #include +#include +#include #include namespace py = pybind11; -void IK(pybind11::module& m) { - m.def("create_ik", []( - dart::dynamics::MetaSkeletonPtr armSkeleton, - aikido::statespace::dart::MetaSkeletonStateSpacePtr armSpace, - aikido::constraint::dart::TSR objectTSR, - dart::dynamics::BodyNode* body_node) -> - aikido::constraint::dart::InverseKinematicsSampleable { - auto goalTSR = std::make_shared(objectTSR); - auto ik = dart::dynamics::InverseKinematics::create(body_node); - ik->setDofs(armSkeleton->getDofs()); - auto rng = std::unique_ptr(new aikido::common::RNGWrapper(0)); - aikido::constraint::dart::InverseKinematicsSampleable ikSampleable(armSpace, - armSkeleton, - goalTSR, - aikido::constraint::dart::createSampleableBounds(armSpace, std::move(rng)), - ik, - 10); - return ikSampleable; - }); +void IK(pybind11::module& m) +{ + m.def( + "create_ik", + [](dart::dynamics::MetaSkeletonPtr armSkeleton, + aikido::statespace::dart::MetaSkeletonStateSpacePtr armSpace, + aikido::constraint::dart::TSR objectTSR, + dart::dynamics::BodyNode* body_node) + -> aikido::constraint::dart::InverseKinematicsSampleable { + auto goalTSR + = std::make_shared(objectTSR); + auto ik = dart::dynamics::InverseKinematics::create(body_node); + ik->setDofs(armSkeleton->getDofs()); + auto rng = std::unique_ptr( + new aikido::common::RNGWrapper(0)); + aikido::constraint::dart::InverseKinematicsSampleable ikSampleable( + armSpace, + armSkeleton, + goalTSR, + aikido::constraint::dart::createSampleableBounds( + armSpace, std::move(rng)), + ik, + 10); + return ikSampleable; + }); - py::class_>(m, "IKSampleable") - .def("create_sample_generator", [](aikido::constraint::dart::InverseKinematicsSampleable* self) -> std::unique_ptr { - return self->createSampleGenerator(); - }); + py::class_< + aikido::constraint::dart::InverseKinematicsSampleable, + std::shared_ptr>( + m, "IKSampleable") + .def( + "create_sample_generator", + [](aikido::constraint::dart::InverseKinematicsSampleable* self) + -> std::unique_ptr { + return self->createSampleGenerator(); + }); py::class_(m, "IKSampleGenerator") - .def("can_sample", [](aikido::constraint::SampleGenerator *self) -> bool { - return self->canSample(); - }) - .def("sample", [](aikido::constraint::SampleGenerator *self, - aikido::statespace::dart::MetaSkeletonStateSpacePtr armSpace) -> Eigen::VectorXd { - auto goalState = armSpace->createState(); - bool sampled = self->sample(goalState); - Eigen::VectorXd positions; - if (!sampled) return positions; - armSpace->convertStateToPositions(goalState, positions); - return positions; - }); + .def( + "can_sample", + [](aikido::constraint::SampleGenerator* self) -> bool { + return self->canSample(); + }) + .def( + "sample", + [](aikido::constraint::SampleGenerator* self, + aikido::statespace::dart::MetaSkeletonStateSpacePtr armSpace) + -> Eigen::VectorXd { + auto goalState = armSpace->createState(); + bool sampled = self->sample(goalState); + Eigen::VectorXd positions; + if (!sampled) + return positions; + armSpace->convertStateToPositions(goalState, positions); + return positions; + }); - py::class_>(m, "BodyNode"); + py::class_< + dart::dynamics::BodyNode, + std::shared_ptr>(m, "BodyNode"); } - diff --git a/src/adapy/pr_tsr_py.cpp b/src/adapy/pr_tsr_py.cpp index 5a995b1..11a070a 100644 --- a/src/adapy/pr_tsr_py.cpp +++ b/src/adapy/pr_tsr_py.cpp @@ -1,37 +1,51 @@ #include -#include -#include +#include #include +#include +#include #include -#include namespace py = pybind11; -Eigen::Isometry3d vectorToIsometry(std::vector &poseVector); +Eigen::Isometry3d vectorToIsometry(std::vector& poseVector); Eigen::Isometry3d matrixToIsometry(Eigen::Matrix4d& poseMatrix); -void Pr_Tsr(pybind11::module& m) { +void Pr_Tsr(pybind11::module& m) +{ m.def("get_default_TSR", &pr_tsr::getDefaultCanTSR); - py::class_>(m, "TSR") - .def("get_T0_w", [](aikido::constraint::dart::TSR *self) -> Eigen::Matrix4d { - auto Trans = self->mT0_w.matrix(); - return Trans; - }) - .def("set_T0_w", [](aikido::constraint::dart::TSR *self, Eigen::Matrix4d& pose) -> void { - auto transform = matrixToIsometry(pose); - self->mT0_w = transform; - }) - .def("set_Bw", [](aikido::constraint::dart::TSR *self, Eigen::MatrixXf& Bw) -> void { - self->mBw = Bw.cast(); - }) - .def("get_Tw_e", [](aikido::constraint::dart::TSR *self) -> Eigen::Matrix4d { - auto Trans = self->mTw_e.matrix(); - return Trans; - }) - .def("set_Tw_e", [](aikido::constraint::dart::TSR *self, Eigen::Matrix4d& pose) -> void { - auto transform = matrixToIsometry(pose); - self->mTw_e = transform; - }); + py::class_< + aikido::constraint::dart::TSR, + std::shared_ptr>(m, "TSR") + .def( + "get_T0_w", + [](aikido::constraint::dart::TSR* self) -> Eigen::Matrix4d { + auto Trans = self->mT0_w.matrix(); + return Trans; + }) + .def( + "set_T0_w", + [](aikido::constraint::dart::TSR* self, + Eigen::Matrix4d& pose) -> void { + auto transform = matrixToIsometry(pose); + self->mT0_w = transform; + }) + .def( + "set_Bw", + [](aikido::constraint::dart::TSR* self, Eigen::MatrixXf& Bw) -> void { + self->mBw = Bw.cast(); + }) + .def( + "get_Tw_e", + [](aikido::constraint::dart::TSR* self) -> Eigen::Matrix4d { + auto Trans = self->mTw_e.matrix(); + return Trans; + }) + .def( + "set_Tw_e", + [](aikido::constraint::dart::TSR* self, + Eigen::Matrix4d& pose) -> void { + auto transform = matrixToIsometry(pose); + self->mTw_e = transform; + }); } - diff --git a/src/libada/Ada.cpp b/src/libada/Ada.cpp index 749fb03..f7909ce 100644 --- a/src/libada/Ada.cpp +++ b/src/libada/Ada.cpp @@ -368,23 +368,25 @@ void Ada::update() //============================================================================== TrajectoryPtr Ada::computeJointSpacePath( const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, - const std::vector>& waypoints) { + const std::vector>& waypoints) +{ auto satisfied = std::make_shared(stateSpace); - std::shared_ptr - interpolator = std::make_shared(stateSpace); - std::shared_ptr - traj = std::make_shared(stateSpace, interpolator); + std::shared_ptr interpolator + = std::make_shared(stateSpace); + std::shared_ptr traj + = std::make_shared( + stateSpace, interpolator); - for (auto &waypoint : waypoints) { + for (auto& waypoint : waypoints) + { auto state = stateSpace->createState(); stateSpace->convertPositionsToState(waypoint.second, state); traj->addWaypoint(waypoint.first, state); } - auto timedTrajectory - = std::move(postProcessPath( - traj.get(), satisfied, ada::KunzParams())); + auto timedTrajectory = std::move( + postProcessPath(traj.get(), satisfied, ada::KunzParams())); return std::move(timedTrajectory); } @@ -393,25 +395,28 @@ TrajectoryPtr Ada::computeJointSpacePath( TrajectoryPtr Ada::computeSmoothJointSpacePath( const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, const std::vector>& waypoints, - const CollisionFreePtr& collisionFree) { - // Use the given constraint if one was given. + const CollisionFreePtr& collisionFree) +{ + // Use the given constraint if one was passed. TestablePtr constraint = collisionFree; if (!constraint) constraint = std::make_shared(stateSpace); - std::shared_ptr - interpolator = std::make_shared(stateSpace); - std::shared_ptr - traj = std::make_shared(stateSpace, interpolator); + std::shared_ptr interpolator + = std::make_shared(stateSpace); + std::shared_ptr traj + = std::make_shared( + stateSpace, interpolator); - for (auto &waypoint : waypoints) { + for (auto& waypoint : waypoints) + { auto state = stateSpace->createState(); stateSpace->convertPositionsToState(waypoint.second, state); traj->addWaypoint(waypoint.first, state); } - auto smoothTrajectory - = postProcessPath(traj.get(), constraint, ParabolicSmoother::Params()); + auto smoothTrajectory = postProcessPath( + traj.get(), constraint, ParabolicSmoother::Params()); return std::move(smoothTrajectory); } From 8a419ecfd5150bae00d9784c64090173d9678ff9 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Feb 2021 19:02:54 -0800 Subject: [PATCH 10/38] Think I fixed ada_py -> compute_retime_path. --- src/adapy/ada_py.cpp | 42 ++++++++++++++++++++++++++---------------- 1 file changed, 26 insertions(+), 16 deletions(-) diff --git a/src/adapy/ada_py.cpp b/src/adapy/ada_py.cpp index 87b83bc..ba57153 100644 --- a/src/adapy/ada_py.cpp +++ b/src/adapy/ada_py.cpp @@ -7,9 +7,13 @@ #include #include "libada/Ada.hpp" +#include "aikido/constraint/Satisfied.hpp" +#include "aikido/planner/kunzretimer/KunzRetimer.hpp" #include "aikido/statespace/ScopedState.hpp" namespace py = pybind11; +using aikido::planner::kunzretimer::KunzRetimer; + void Ada(pybind11::module& m) { //====================================LIBADA========================================================== @@ -127,22 +131,28 @@ void Ada(pybind11::module& m) const dart::dynamics::MetaSkeletonPtr& armSkeleton, aikido::trajectory::TrajectoryPtr trajectory_ptr) -> aikido::trajectory::TrajectoryPtr { - return self->retimePath(armSkeleton, trajectory_ptr.get()); - }) - .def( - "plan_to_configuration", - [](ada::Ada* self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& - armSpace, - const dart::dynamics::MetaSkeletonPtr& armSkeleton, - const Eigen::VectorXd& configuration) - -> aikido::trajectory::TrajectoryPtr { - auto state = armSpace->createState(); - armSpace->convertPositionsToState(configuration, state); - auto trajectory = self->planToConfiguration( - armSpace, armSkeleton, state, nullptr, 10); - return trajectory; - }) + auto satisfied = std::make_shared( + trajectory_ptr->getStateSpace()); + return self->postProcessPath( + trajectory_ptr.get(), + satisfied, + ada::KunzParams(), + armSkeleton->getVelocityUpperLimits(), + armSkeleton->getAccelerationUpperLimits()); + }) + .def( + "plan_to_configuration", + [](ada::Ada* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& armSpace, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + const Eigen::VectorXd& configuration) + -> aikido::trajectory::TrajectoryPtr { + auto state = armSpace->createState(); + armSpace->convertPositionsToState(configuration, state); + auto trajectory = self->planToConfiguration( + armSpace, armSkeleton, state, nullptr, 10); + return trajectory; + }) .def( "execute_trajectory", [](ada::Ada* self, From 53bb3b67e92dafd3fc2fa60bc598b9ab81fcc9a9 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Feb 2021 19:31:53 -0800 Subject: [PATCH 11/38] Add ICAROS executePreshape(). --- include/libada/AdaHand.hpp | 2 ++ src/adapy/ada_py.cpp | 31 ++++++++++++++++--------------- src/libada/AdaHand.cpp | 12 +++++++++--- 3 files changed, 27 insertions(+), 18 deletions(-) diff --git a/include/libada/AdaHand.hpp b/include/libada/AdaHand.hpp index 167acfb..26bb686 100644 --- a/include/libada/AdaHand.hpp +++ b/include/libada/AdaHand.hpp @@ -83,6 +83,8 @@ class AdaHand : public aikido::robot::Hand // Documentation inherited. std::future executePreshape(const std::string& preshapeName) override; + std::future executePreshape(const Eigen::Vector2d& preshape); + // Documentation inherited. void step(const std::chrono::system_clock::time_point& timepoint) override; diff --git a/src/adapy/ada_py.cpp b/src/adapy/ada_py.cpp index ba57153..254a694 100644 --- a/src/adapy/ada_py.cpp +++ b/src/adapy/ada_py.cpp @@ -140,19 +140,20 @@ void Ada(pybind11::module& m) armSkeleton->getVelocityUpperLimits(), armSkeleton->getAccelerationUpperLimits()); }) - .def( - "plan_to_configuration", - [](ada::Ada* self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& armSpace, - const dart::dynamics::MetaSkeletonPtr& armSkeleton, - const Eigen::VectorXd& configuration) - -> aikido::trajectory::TrajectoryPtr { - auto state = armSpace->createState(); - armSpace->convertPositionsToState(configuration, state); - auto trajectory = self->planToConfiguration( - armSpace, armSkeleton, state, nullptr, 10); - return trajectory; - }) + .def( + "plan_to_configuration", + [](ada::Ada* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& + armSpace, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + const Eigen::VectorXd& configuration) + -> aikido::trajectory::TrajectoryPtr { + auto state = armSpace->createState(); + armSpace->convertPositionsToState(configuration, state); + auto trajectory = self->planToConfiguration( + armSpace, armSkeleton, state, nullptr, 10); + return trajectory; + }) .def( "execute_trajectory", [](ada::Ada* self, @@ -171,12 +172,12 @@ void Ada(pybind11::module& m) [](ada::Ada* self, const std::string& topicName, const std::string& baseFrameName) - -> std::shared_ptr { + -> std::shared_ptr { int argc = 1; char* argv[] = {"adapy"}; ros::init(argc, argv, "ada"); auto viewer - = std::make_shared( + = std::make_shared( self->getWorld(), topicName, baseFrameName); viewer->setAutoUpdate(true); return viewer; diff --git a/src/libada/AdaHand.cpp b/src/libada/AdaHand.cpp index 5becec9..f885011 100644 --- a/src/libada/AdaHand.cpp +++ b/src/libada/AdaHand.cpp @@ -264,8 +264,6 @@ void AdaHand::ungrab() //============================================================================== std::future AdaHand::executePreshape(const std::string& preshapeName) { - using aikido::constraint::Satisfied; - boost::optional preshape = getPreshape(preshapeName); if (!preshape) @@ -276,8 +274,16 @@ std::future AdaHand::executePreshape(const std::string& preshapeName) throw std::runtime_error(message.str()); } + return executePreshape(preshape.get()); +} + +//============================================================================== +std::future AdaHand::executePreshape(const Eigen::Vector2d& preshape) +{ + using aikido::constraint::Satisfied; + auto goalState = mSpace->createState(); - mSpace->convertPositionsToState(preshape.get(), goalState); + mSpace->convertPositionsToState(preshape, goalState); auto satisfied = std::make_shared(mSpace); From fecacab6ae158abe086503698021a46b65201a8e Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Feb 2021 19:38:10 -0800 Subject: [PATCH 12/38] Holy cow, think it builds? --- src/adapy/ada_py.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/adapy/ada_py.cpp b/src/adapy/ada_py.cpp index 254a694..07d7b01 100644 --- a/src/adapy/ada_py.cpp +++ b/src/adapy/ada_py.cpp @@ -178,7 +178,7 @@ void Ada(pybind11::module& m) ros::init(argc, argv, "ada"); auto viewer = std::make_shared( - self->getWorld(), topicName, baseFrameName); + topicName, baseFrameName, self->getWorld()); viewer->setAutoUpdate(true); return viewer; }); From bf923c964edabb1a0a9679f8e5e5e31930d74096 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Feb 2021 21:42:07 -0800 Subject: [PATCH 13/38] Move to Python 2.7 because current stack. --- CMakeLists.txt | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f2bb381..d79d69c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,9 +2,7 @@ cmake_minimum_required(VERSION 3.10.0) project(libada) set(library_VERSION 0.3.0) -if(NOT ADAPY_PYTHON_VERSION) - set(ADAPY_PYTHON_VERSION 3.6) -endif() +set(ADAPY_PYTHON_VERSION 2.7) find_package(PythonInterp ${ADAPY_PYTHON_VERSION} REQUIRED) execute_process(COMMAND ${PYTHON_EXECUTABLE} -c @@ -22,7 +20,7 @@ find_package(PythonLibs ${ADAPY_PYTHON_VERSION} REQUIRED) # Find pybind11 # Needs to set PYBIND11_PYTHON_VERSION before finding pybind11 -set(PYBIND11_PYTHON_VERSION ${ADAPY_PYTHON_VERSION}) +set(PYBIND11_PYTHON_VERSION ${ADAPY_PYTHON_VERSION} CACHE STRING "") find_package(pybind11 2.2.0 REQUIRED) @@ -73,8 +71,6 @@ find_package(aikido 0.3.0 REQUIRED rviz ) -find_package(PythonLibs 3.6 REQUIRED) - find_package(ada_description REQUIRED) find_package(Boost REQUIRED python) @@ -207,7 +203,7 @@ install(TARGETS libada EXPORT libadaConfig install(DIRECTORY include/ DESTINATION include) install(TARGETS adapy - LIBRARY DESTINATION lib/python3.6/dist-packages) + LIBRARY DESTINATION lib/python2.7/dist-packages) # This makes the project importable from the install directory install(EXPORT libadaConfig DESTINATION share/libada/cmake) From 9fbe0e732758d06d536da56a929f04c55dad96b6 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Feb 2021 21:50:32 -0800 Subject: [PATCH 14/38] Fix exec waiting in bindings, clean up simple_trajectories. --- src/adapy/ada_py.cpp | 11 +++++------ src/scripts/simple_trajectories.py | 12 ++++++++++-- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/adapy/ada_py.cpp b/src/adapy/ada_py.cpp index 07d7b01..d4b23b0 100644 --- a/src/adapy/ada_py.cpp +++ b/src/adapy/ada_py.cpp @@ -160,12 +160,11 @@ void Ada(pybind11::module& m) const aikido::trajectory::TrajectoryPtr& trajectory) -> void { auto future = self->executeTrajectory(trajectory); - // if (!future.valid()) { - // std::__throw_future_error(0); - // } - // - // future.wait(); - // ros::Duration(15).sleep(); + if (!future.valid()) { + std::__throw_future_error(0); + } + + future.wait(); }) .def( "start_viewer", diff --git a/src/scripts/simple_trajectories.py b/src/scripts/simple_trajectories.py index 3b3905f..cbcea95 100755 --- a/src/scripts/simple_trajectories.py +++ b/src/scripts/simple_trajectories.py @@ -4,7 +4,6 @@ import rospy import pdb -pdb.set_trace() rospy.init_node("adapy_simple_traj") rate = rospy.Rate(10) @@ -38,6 +37,15 @@ waypoints = [(0.0, positions), (1.0, positions2), (2.0, positions3), (3.0, positions4)] traj = ada.compute_joint_space_path(arm_state_space, waypoints) + + print("") + print("CONTINUE TO EXECUTE") + print("") + pdb.set_trace() + ada.execute_trajectory(traj) - rospy.sleep(1.0) + print("") + print("DONE! CONTINUE TO EXIT") + print("") + pdb.set_trace() From 7fbecd635d403fce67d99408ee14c8e47138ba0f Mon Sep 17 00:00:00 2001 From: sniyaz Date: Mon, 1 Mar 2021 18:48:31 -0800 Subject: [PATCH 15/38] Run make format. --- src/adapy/ada_py.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/adapy/ada_py.cpp b/src/adapy/ada_py.cpp index d4b23b0..11039ca 100644 --- a/src/adapy/ada_py.cpp +++ b/src/adapy/ada_py.cpp @@ -160,7 +160,8 @@ void Ada(pybind11::module& m) const aikido::trajectory::TrajectoryPtr& trajectory) -> void { auto future = self->executeTrajectory(trajectory); - if (!future.valid()) { + if (!future.valid()) + { std::__throw_future_error(0); } From 2075654c08a097a661f28071a2e1d962bcc29a33 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Mon, 1 Mar 2021 19:04:35 -0800 Subject: [PATCH 16/38] Fix wait() in execute_preshape(). --- src/adapy/ada_py.cpp | 9 +++++++-- src/scripts/simple_trajectories.py | 3 +++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/adapy/ada_py.cpp b/src/adapy/ada_py.cpp index 11039ca..6906a54 100644 --- a/src/adapy/ada_py.cpp +++ b/src/adapy/ada_py.cpp @@ -202,8 +202,13 @@ void Ada(pybind11::module& m) "execute_preshape", [](ada::AdaHand* self, const Eigen::Vector2d& d) -> void { auto future = self->executePreshape(d); - // future.wait(); - // ros::Duration(5).sleep(); + + if (!future.valid()) + { + std::__throw_future_error(0); + } + + future.wait(); }) .def( "get_endeffector_transform", diff --git a/src/scripts/simple_trajectories.py b/src/scripts/simple_trajectories.py index cbcea95..13afe5c 100755 --- a/src/scripts/simple_trajectories.py +++ b/src/scripts/simple_trajectories.py @@ -44,6 +44,9 @@ pdb.set_trace() ada.execute_trajectory(traj) + # Also close the hand. + preshape = [1.1, 1.1] + ada.get_hand().execute_preshape(preshape); print("") print("DONE! CONTINUE TO EXIT") From aba5d3fcc2f2268f1543ff9d6075bfd79b53540a Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Thu, 11 Mar 2021 20:13:38 -0800 Subject: [PATCH 17/38] Fixed warnings, now works and tested on real robot at PRL, see PR comments for some specifics --- CMakeLists.txt | 5 ++-- include/libada/Ada.hpp | 2 +- include/libada/detail/Ada-impl.hpp | 8 ++++-- src/adapy/ada_py.cpp | 39 +++++++++++++++++++++++++++--- src/libada/Ada.cpp | 4 +-- src/scripts/simple_trajectories.py | 29 +++++++++++++++++++++- 6 files changed, 74 insertions(+), 13 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d79d69c..26cee28 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.10.0) project(libada) set(library_VERSION 0.3.0) -set(ADAPY_PYTHON_VERSION 2.7) +set(ADAPY_PYTHON_VERSION 3.8) find_package(PythonInterp ${ADAPY_PYTHON_VERSION} REQUIRED) execute_process(COMMAND ${PYTHON_EXECUTABLE} -c @@ -23,7 +23,6 @@ find_package(PythonLibs ${ADAPY_PYTHON_VERSION} REQUIRED) set(PYBIND11_PYTHON_VERSION ${ADAPY_PYTHON_VERSION} CACHE STRING "") find_package(pybind11 2.2.0 REQUIRED) - option(LIBADA_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") @@ -203,7 +202,7 @@ install(TARGETS libada EXPORT libadaConfig install(DIRECTORY include/ DESTINATION include) install(TARGETS adapy - LIBRARY DESTINATION lib/python2.7/dist-packages) + LIBRARY DESTINATION lib/python3/dist-packages) # This makes the project importable from the install directory install(EXPORT libadaConfig DESTINATION share/libada/cmake) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 1d76ed5..17ad877 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -89,7 +89,7 @@ class Ada final : public aikido::robot::Robot const dart::common::Uri& adaUrdfUri = defaultAdaUrdfUri, const dart::common::Uri& adaSrdfUri = defaultAdaSrdfUri, const std::string& endEffectorName = "j2n6s200_end_effector", - const std::string& armTrajectoryExecutorName = "trajectory_controller", + const std::string& armTrajectoryExecutorName = "rewd_trajectory_controller", const ::ros::NodeHandle* node = nullptr, aikido::common::RNG::result_type rngSeed = std::random_device{}(), const dart::common::ResourceRetrieverPtr& retriever diff --git a/include/libada/detail/Ada-impl.hpp b/include/libada/detail/Ada-impl.hpp index 22c4cf2..3df7ecf 100644 --- a/include/libada/detail/Ada-impl.hpp +++ b/include/libada/detail/Ada-impl.hpp @@ -10,17 +10,21 @@ aikido::trajectory::UniqueSplinePtr Ada::postProcessPath( const Eigen::VectorXd& velocityLimits, const Eigen::VectorXd& accelerationLimits) { + // Don't plan above 70% hard velocity limit + // Unless requested explicitly + const double DEFAULT_LIMITS_BUFFER = 0.7; + bool velLimitsInvalid = (velocityLimits.squaredNorm() == 0.0) || velocityLimits.size() != getVelocityLimits().size(); auto sentVelocityLimits - = velLimitsInvalid ? getVelocityLimits() : velocityLimits; + = velLimitsInvalid ? DEFAULT_LIMITS_BUFFER * getVelocityLimits() : velocityLimits; bool accLimitsInvalid = (accelerationLimits.squaredNorm() == 0.0) || accelerationLimits.size() != getAccelerationLimits().size(); auto sentAccelerationLimits - = accLimitsInvalid ? getAccelerationLimits() : accelerationLimits; + = accLimitsInvalid ? DEFAULT_LIMITS_BUFFER * getAccelerationLimits() : accelerationLimits; return mRobot->postProcessPath( sentVelocityLimits, diff --git a/src/adapy/ada_py.cpp b/src/adapy/ada_py.cpp index 6906a54..045b4fc 100644 --- a/src/adapy/ada_py.cpp +++ b/src/adapy/ada_py.cpp @@ -32,6 +32,9 @@ void Ada(pybind11::module& m) .def( "start_trajectory_executor", [](ada::Ada* self) -> void { self->startTrajectoryExecutor(); }) + .def( + "stop_trajectory_executor", + [](ada::Ada* self) -> void { self->stopTrajectoryExecutor(); }) .def( "get_name", [](ada::Ada* self) -> std::string { return self->getName(); }) @@ -164,8 +167,9 @@ void Ada(pybind11::module& m) { std::__throw_future_error(0); } - future.wait(); + // Throw any exceptions + future.get(); }) .def( "start_viewer", @@ -173,9 +177,6 @@ void Ada(pybind11::module& m) const std::string& topicName, const std::string& baseFrameName) -> std::shared_ptr { - int argc = 1; - char* argv[] = {"adapy"}; - ros::init(argc, argv, "ada"); auto viewer = std::make_shared( topicName, baseFrameName, self->getWorld()); @@ -209,6 +210,36 @@ void Ada(pybind11::module& m) } future.wait(); + // Throw any exceptions + future.get(); + }) + .def( + "open", + [](ada::AdaHand* self) -> void { + auto future = self->executePreshape("open"); + + if (!future.valid()) + { + std::__throw_future_error(0); + } + + future.wait(); + // Throw any exceptions + future.get(); + }) + .def( + "close", + [](ada::AdaHand* self) -> void { + auto future = self->executePreshape("closed"); + + if (!future.valid()) + { + std::__throw_future_error(0); + } + + future.wait(); + // Throw any exceptions + future.get(); }) .def( "get_endeffector_transform", diff --git a/src/libada/Ada.cpp b/src/libada/Ada.cpp index f7909ce..5230928 100644 --- a/src/libada/Ada.cpp +++ b/src/libada/Ada.cpp @@ -388,7 +388,7 @@ TrajectoryPtr Ada::computeJointSpacePath( auto timedTrajectory = std::move( postProcessPath(traj.get(), satisfied, ada::KunzParams())); - return std::move(timedTrajectory); + return timedTrajectory; } //============================================================================== @@ -418,7 +418,7 @@ TrajectoryPtr Ada::computeSmoothJointSpacePath( auto smoothTrajectory = postProcessPath( traj.get(), constraint, ParabolicSmoother::Params()); - return std::move(smoothTrajectory); + return smoothTrajectory; } //============================================================================== diff --git a/src/scripts/simple_trajectories.py b/src/scripts/simple_trajectories.py index 13afe5c..4953281 100755 --- a/src/scripts/simple_trajectories.py +++ b/src/scripts/simple_trajectories.py @@ -6,10 +6,15 @@ import pdb rospy.init_node("adapy_simple_traj") +from moveit_ros_planning_interface._moveit_roscpp_initializer import roscpp_init +roscpp_init('adapy_simple_traj', []) rate = rospy.Rate(10) +is_sim=False if not rospy.is_shutdown(): - ada = adapy.Ada(True) + ada = adapy.Ada(is_sim) + if not is_sim: + ada.start_trajectory_executor() 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] @@ -36,7 +41,9 @@ positions4[2] += 0.6 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.compute_joint_space_path(arm_state_space, waypoints) + traj_rev = ada.compute_joint_space_path(arm_state_space, waypoints_rev) print("") print("CONTINUE TO EXECUTE") @@ -44,11 +51,31 @@ pdb.set_trace() ada.execute_trajectory(traj) + + print("") + print("CLOSING HAND") + print("") # Also close the hand. preshape = [1.1, 1.1] ada.get_hand().execute_preshape(preshape); + print("") + print("CONTINUE TO EXECUTE REVERSE") + print("") + pdb.set_trace() + + ada.execute_trajectory(traj_rev) + + print("") + print("OPENING HAND") + print("") + # Also close the hand. + preshape = [1.1, 1.1] + ada.get_hand().open(); + print("") print("DONE! CONTINUE TO EXIT") print("") pdb.set_trace() + if not is_sim: + ada.stop_trajectory_executor() From 222fc4f491350b301ee4e677f0d4ef2fe7e07c1d Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Thu, 11 Mar 2021 20:46:01 -0800 Subject: [PATCH 18/38] Resolved Hejia's Comments --- include/libada/Ada.hpp | 2 +- src/adapy/ada_py.cpp | 4 ++-- src/libada/Ada.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 17ad877..2165294 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -176,7 +176,7 @@ class Ada final : public aikido::robot::Robot // Runs step with current time. void update(); - aikido::trajectory::TrajectoryPtr computeJointSpacePath( + aikido::trajectory::TrajectoryPtr computeRetimedJointSpacePath( const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, const std::vector>& waypoints); diff --git a/src/adapy/ada_py.cpp b/src/adapy/ada_py.cpp index 045b4fc..e42ee7a 100644 --- a/src/adapy/ada_py.cpp +++ b/src/adapy/ada_py.cpp @@ -114,7 +114,7 @@ void Ada(pybind11::module& m) stateSpace, const std::vector>& waypoints) -> aikido::trajectory::TrajectoryPtr { - return self->computeJointSpacePath(stateSpace, waypoints); + return self->computeRetimedJointSpacePath(stateSpace, waypoints); }) .def( "compute_smooth_joint_space_path", @@ -132,7 +132,7 @@ void Ada(pybind11::module& m) "compute_retime_path", [](ada::Ada* self, const dart::dynamics::MetaSkeletonPtr& armSkeleton, - aikido::trajectory::TrajectoryPtr trajectory_ptr) + aikido::trajectory::InterpolatedPtr trajectory_ptr) -> aikido::trajectory::TrajectoryPtr { auto satisfied = std::make_shared( trajectory_ptr->getStateSpace()); diff --git a/src/libada/Ada.cpp b/src/libada/Ada.cpp index 5230928..3f8f3e1 100644 --- a/src/libada/Ada.cpp +++ b/src/libada/Ada.cpp @@ -366,7 +366,7 @@ void Ada::update() } //============================================================================== -TrajectoryPtr Ada::computeJointSpacePath( +TrajectoryPtr Ada::computeRetimedJointSpacePath( const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, const std::vector>& waypoints) { From 3d1c2302ed8f97e7a699b056309ee5f8fd123e12 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Thu, 11 Mar 2021 20:58:00 -0800 Subject: [PATCH 19/38] Removed unused Boost library requirement --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 26cee28..44826aa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,7 +72,7 @@ find_package(aikido 0.3.0 REQUIRED find_package(ada_description REQUIRED) -find_package(Boost REQUIRED python) +find_package(Boost REQUIRED) find_package(controller_manager_msgs REQUIRED) From 8b6ca6eacaf99d7e72bdae2138d537d78130e617 Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Mon, 15 Mar 2021 15:12:16 -0700 Subject: [PATCH 20/38] Moved python bindings to proper subfolder --- CMakeLists.txt | 101 ++++++++++-------- {src => python}/adapy/ada_py.cpp | 0 {src => python}/adapy/adapy.cpp | 0 {src => python}/adapy/aikido_py.cpp | 0 {src => python}/adapy/dart_py.cpp | 0 {src => python}/adapy/ik_py.cpp | 0 {src => python}/adapy/pr_tsr_py.cpp | 0 .../tests}/simple_trajectories.py | 0 .../tests}/test_collisions.py | 0 9 files changed, 55 insertions(+), 46 deletions(-) rename {src => python}/adapy/ada_py.cpp (100%) rename {src => python}/adapy/adapy.cpp (100%) rename {src => python}/adapy/aikido_py.cpp (100%) rename {src => python}/adapy/dart_py.cpp (100%) rename {src => python}/adapy/ik_py.cpp (100%) rename {src => python}/adapy/pr_tsr_py.cpp (100%) rename {src/scripts => python/tests}/simple_trajectories.py (100%) rename {src/scripts => python/tests}/test_collisions.py (100%) mode change 100644 => 100755 diff --git a/CMakeLists.txt b/CMakeLists.txt index 44826aa..202614c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,33 +2,40 @@ cmake_minimum_required(VERSION 3.10.0) project(libada) set(library_VERSION 0.3.0) -set(ADAPY_PYTHON_VERSION 3.8) - -find_package(PythonInterp ${ADAPY_PYTHON_VERSION} REQUIRED) -execute_process(COMMAND ${PYTHON_EXECUTABLE} -c - "from distutils.sysconfig import get_python_lib;\ - print(get_python_lib(plat_specific=True, prefix=''))" - OUTPUT_VARIABLE PYTHON_SITE_PACKAGES - OUTPUT_STRIP_TRAILING_WHITESPACE -) -if(NOT PythonInterp_FOUND) - message(WARNING "Failed to find PythonInterp.") - return() -endif() - -find_package(PythonLibs ${ADAPY_PYTHON_VERSION} REQUIRED) - -# Find pybind11 -# Needs to set PYBIND11_PYTHON_VERSION before finding pybind11 -set(PYBIND11_PYTHON_VERSION ${ADAPY_PYTHON_VERSION} CACHE STRING "") -find_package(pybind11 2.2.0 REQUIRED) - option(LIBADA_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") set(CMAKE_CXX_STANDARD 14) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") +#================================================================================ +# Python setup +# + +set(ADAPY_PYTHON_VERSION 3.8) +find_package(PythonInterp ${ADAPY_PYTHON_VERSION}) + +if(PythonInterp_FOUND) + execute_process(COMMAND ${PYTHON_EXECUTABLE} -c + "from distutils.sysconfig import get_python_lib;\ + print(get_python_lib(plat_specific=True, prefix=''))" + OUTPUT_VARIABLE PYTHON_SITE_PACKAGES + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + + find_package(PythonLibs ${ADAPY_PYTHON_VERSION}) + + # Find pybind11 + # Needs to set PYBIND11_PYTHON_VERSION before finding pybind11 + set(PYBIND11_PYTHON_VERSION ${ADAPY_PYTHON_VERSION} CACHE STRING "") + find_package(pybind11 2.2.0) +endif() + +if(NOT pybind11_FOUND) + message(WARNING "Could not load pybind11. Cannot build python bindings.") + return() +endif() + #================================================================================ # CodeCov setup # @@ -139,30 +146,31 @@ target_link_libraries(libada #============================================================================= #Pybind +if(pybind11_FOUND) + pybind11_add_module(adapy + MODULE + python/adapy/adapy.cpp + python/adapy/ada_py.cpp + python/adapy/aikido_py.cpp + python/adapy/dart_py.cpp + python/adapy/pr_tsr_py.cpp + python/adapy/ik_py.cpp + ) -pybind11_add_module(adapy - MODULE - src/adapy/adapy.cpp - src/adapy/ada_py.cpp - src/adapy/aikido_py.cpp - src/adapy/dart_py.cpp - src/adapy/pr_tsr_py.cpp - src/adapy/ik_py.cpp -) - -target_include_directories(adapy - SYSTEM PUBLIC - ${PYTHON_INCLUDE_DIRS} - ${pybind11_INCLUDE_DIRS} - PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR} -) + target_include_directories(adapy + SYSTEM PUBLIC + ${PYTHON_INCLUDE_DIRS} + ${pybind11_INCLUDE_DIRS} + PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} + ) -target_link_libraries(adapy PUBLIC - libada - ${libaikidopy_LIBRARIES} - ${DART_LIBRARIES} -) + target_link_libraries(adapy PUBLIC + libada + ${libaikidopy_LIBRARIES} + ${DART_LIBRARIES} + ) +endif() #================================================================================ # Testing @@ -201,9 +209,6 @@ install(TARGETS libada EXPORT libadaConfig RUNTIME DESTINATION bin) install(DIRECTORY include/ DESTINATION include) -install(TARGETS adapy - LIBRARY DESTINATION lib/python3/dist-packages) - # This makes the project importable from the install directory install(EXPORT libadaConfig DESTINATION share/libada/cmake) @@ -215,6 +220,10 @@ install(FILES "package.xml" DESTINATION "share/libada") #install(DIRECTORY resources DESTINATION share) +if(pybind11_FOUND) + install(TARGETS adapy + LIBRARY DESTINATION lib/python3/dist-packages) +endif() #================================================================================ diff --git a/src/adapy/ada_py.cpp b/python/adapy/ada_py.cpp similarity index 100% rename from src/adapy/ada_py.cpp rename to python/adapy/ada_py.cpp diff --git a/src/adapy/adapy.cpp b/python/adapy/adapy.cpp similarity index 100% rename from src/adapy/adapy.cpp rename to python/adapy/adapy.cpp diff --git a/src/adapy/aikido_py.cpp b/python/adapy/aikido_py.cpp similarity index 100% rename from src/adapy/aikido_py.cpp rename to python/adapy/aikido_py.cpp diff --git a/src/adapy/dart_py.cpp b/python/adapy/dart_py.cpp similarity index 100% rename from src/adapy/dart_py.cpp rename to python/adapy/dart_py.cpp diff --git a/src/adapy/ik_py.cpp b/python/adapy/ik_py.cpp similarity index 100% rename from src/adapy/ik_py.cpp rename to python/adapy/ik_py.cpp diff --git a/src/adapy/pr_tsr_py.cpp b/python/adapy/pr_tsr_py.cpp similarity index 100% rename from src/adapy/pr_tsr_py.cpp rename to python/adapy/pr_tsr_py.cpp diff --git a/src/scripts/simple_trajectories.py b/python/tests/simple_trajectories.py similarity index 100% rename from src/scripts/simple_trajectories.py rename to python/tests/simple_trajectories.py diff --git a/src/scripts/test_collisions.py b/python/tests/test_collisions.py old mode 100644 new mode 100755 similarity index 100% rename from src/scripts/test_collisions.py rename to python/tests/test_collisions.py From f02d54fd6a9d2994c918195ba4c7d62619730f9e Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Mon, 15 Mar 2021 17:04:24 -0700 Subject: [PATCH 21/38] Address comments in PR --- CMakeLists.txt | 11 ++++----- python/adapy/aikido_py.cpp | 23 +++++-------------- python/adapy/ik_py.cpp | 1 + python/adapy/pr_tsr_py.cpp | 6 ++--- python/tests/simple_trajectories.py | 17 +++++++------- python/tests/test_collisions.py | 19 +++++++++------ src/{libada => }/Ada.cpp | 0 ...maticSimulationPositionCommandExecutor.cpp | 0 src/{libada => }/AdaHand.cpp | 0 ...maticSimulationPositionCommandExecutor.cpp | 0 src/{libada => }/util.cpp | 0 11 files changed, 34 insertions(+), 43 deletions(-) rename src/{libada => }/Ada.cpp (100%) rename src/{libada => }/AdaFingerKinematicSimulationPositionCommandExecutor.cpp (100%) rename src/{libada => }/AdaHand.cpp (100%) rename src/{libada => }/AdaHandKinematicSimulationPositionCommandExecutor.cpp (100%) rename src/{libada => }/util.cpp (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 202614c..047a332 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -103,11 +103,11 @@ endif() # set(sources - src/libada/AdaFingerKinematicSimulationPositionCommandExecutor.cpp - src/libada/AdaHandKinematicSimulationPositionCommandExecutor.cpp - src/libada/AdaHand.cpp - src/libada/Ada.cpp - src/libada/util.cpp + src/AdaFingerKinematicSimulationPositionCommandExecutor.cpp + src/AdaHandKinematicSimulationPositionCommandExecutor.cpp + src/AdaHand.cpp + src/Ada.cpp + src/util.cpp ) add_library(libada SHARED ${sources}) @@ -167,7 +167,6 @@ if(pybind11_FOUND) target_link_libraries(adapy PUBLIC libada - ${libaikidopy_LIBRARIES} ${DART_LIBRARIES} ) endif() diff --git a/python/adapy/aikido_py.cpp b/python/adapy/aikido_py.cpp index 4eb926f..2de09a5 100644 --- a/python/adapy/aikido_py.cpp +++ b/python/adapy/aikido_py.cpp @@ -1,12 +1,12 @@ #include #include #include +#include #include #include #include #include #include -#include #include #include #include @@ -50,11 +50,9 @@ void Aikido(pybind11::module& m) -> std::shared_ptr<::dart::dynamics::Skeleton> { auto transform = vectorToIsometry(objectPose); - dart::utils::DartLoader urdfLoader; - const auto resourceRetriever - = std::make_shared(); const auto skeleton - = urdfLoader.parseSkeleton(uri, resourceRetriever); + = aikido::io::loadSkeletonFromURDF( + std::make_shared(), uri); if (!skeleton) throw std::runtime_error("unable to load '" + uri + "'"); @@ -73,11 +71,9 @@ void Aikido(pybind11::module& m) -> std::shared_ptr<::dart::dynamics::Skeleton> { auto transform = matrixToIsometry(objectPose); - dart::utils::DartLoader urdfLoader; - const auto resourceRetriever - = std::make_shared(); const auto skeleton - = urdfLoader.parseSkeleton(uri, resourceRetriever); + = aikido::io::loadSkeletonFromURDF( + std::make_shared(), uri); if (!skeleton) throw std::runtime_error("unable to load '" + uri + "'"); @@ -98,14 +94,7 @@ void Aikido(pybind11::module& m) "get_skeleton", [](aikido::planner::World* self, int i) -> dart::dynamics::SkeletonPtr { - if (i + 1 <= int(self->getNumSkeletons())) - { - return self->getSkeleton(i); - } - else - { - return nullptr; - } + return self->getSkeleton(i); }); py::class_< aikido::rviz::InteractiveMarkerViewer, diff --git a/python/adapy/ik_py.cpp b/python/adapy/ik_py.cpp index 9ba839d..ea95603 100644 --- a/python/adapy/ik_py.cpp +++ b/python/adapy/ik_py.cpp @@ -61,6 +61,7 @@ void IK(pybind11::module& m) -> Eigen::VectorXd { auto goalState = armSpace->createState(); bool sampled = self->sample(goalState); + // Init to length-0 vector Eigen::VectorXd positions; if (!sampled) return positions; diff --git a/python/adapy/pr_tsr_py.cpp b/python/adapy/pr_tsr_py.cpp index 11a070a..a5eb181 100644 --- a/python/adapy/pr_tsr_py.cpp +++ b/python/adapy/pr_tsr_py.cpp @@ -20,8 +20,7 @@ void Pr_Tsr(pybind11::module& m) .def( "get_T0_w", [](aikido::constraint::dart::TSR* self) -> Eigen::Matrix4d { - auto Trans = self->mT0_w.matrix(); - return Trans; + return self->mT0_w.matrix(); }) .def( "set_T0_w", @@ -38,8 +37,7 @@ void Pr_Tsr(pybind11::module& m) .def( "get_Tw_e", [](aikido::constraint::dart::TSR* self) -> Eigen::Matrix4d { - auto Trans = self->mTw_e.matrix(); - return Trans; + return self->mTw_e.matrix(); }) .def( "set_Tw_e", diff --git a/python/tests/simple_trajectories.py b/python/tests/simple_trajectories.py index 4953281..b8ed78b 100755 --- a/python/tests/simple_trajectories.py +++ b/python/tests/simple_trajectories.py @@ -4,12 +4,12 @@ import rospy import pdb +from moveit_ros_planning_interface._moveit_roscpp_initializer import roscpp_init rospy.init_node("adapy_simple_traj") -from moveit_ros_planning_interface._moveit_roscpp_initializer import roscpp_init roscpp_init('adapy_simple_traj', []) rate = rospy.Rate(10) -is_sim=False +is_sim = False if not rospy.is_shutdown(): ada = adapy.Ada(is_sim) @@ -40,8 +40,10 @@ positions4[1] -= 0.4 positions4[2] += 0.6 - 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)] + 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.compute_joint_space_path(arm_state_space, waypoints) traj_rev = ada.compute_joint_space_path(arm_state_space, waypoints_rev) @@ -55,9 +57,8 @@ print("") print("CLOSING HAND") print("") - # Also close the hand. preshape = [1.1, 1.1] - ada.get_hand().execute_preshape(preshape); + ada.get_hand().execute_preshape(preshape) print("") print("CONTINUE TO EXECUTE REVERSE") @@ -69,9 +70,7 @@ print("") print("OPENING HAND") print("") - # Also close the hand. - preshape = [1.1, 1.1] - ada.get_hand().open(); + ada.get_hand().open() print("") print("DONE! CONTINUE TO EXIT") diff --git a/python/tests/test_collisions.py b/python/tests/test_collisions.py index d10351d..4aade1a 100755 --- a/python/tests/test_collisions.py +++ b/python/tests/test_collisions.py @@ -24,18 +24,22 @@ arm_state_space = ada.get_arm_state_space() -collision_free_constraint = ada.set_up_collision_detection(arm_state_space, arm_skeleton, [can, table]) -full_collision_free_constraint = ada.get_full_collision_constraint(arm_state_space, arm_skeleton, collision_free_constraint) +collision_free_constraint = ada.set_up_collision_detection( + arm_state_space, arm_skeleton, [can, table]) +full_collision_free_constraint = ada.get_full_collision_constraint( + arm_state_space, arm_skeleton, collision_free_constraint) # test original position -safe = full_collision_free_constraint.is_satisfied(arm_state_space, arm_skeleton, positions) +safe = full_collision_free_constraint.is_satisfied( + arm_state_space, arm_skeleton, positions) if not safe: print("State state is not safe!") # test self-collision positions2 = positions.copy() positions2[1] = 0.4 -safe = full_collision_free_constraint.is_satisfied(arm_state_space, arm_skeleton, positions2) +safe = full_collision_free_constraint.is_satisfied( + arm_state_space, arm_skeleton, positions2) if not safe: print("Self-collision!") @@ -43,14 +47,15 @@ positions3 = positions.copy() positions3[1] -= 0.4 positions3[2] += 0.6 -safe = full_collision_free_constraint.is_satisfied(arm_state_space, arm_skeleton, positions3) +safe = full_collision_free_constraint.is_satisfied( + arm_state_space, arm_skeleton, positions3) if not safe: print("Arm body is in collision!") # test robot hand collision positions4 = positions.copy() positions4[1] = 2.1 -safe = full_collision_free_constraint.is_satisfied(arm_state_space, arm_skeleton, positions4) +safe = full_collision_free_constraint.is_satisfied( + arm_state_space, arm_skeleton, positions4) if not safe: print("Hand is in collision!") - diff --git a/src/libada/Ada.cpp b/src/Ada.cpp similarity index 100% rename from src/libada/Ada.cpp rename to src/Ada.cpp diff --git a/src/libada/AdaFingerKinematicSimulationPositionCommandExecutor.cpp b/src/AdaFingerKinematicSimulationPositionCommandExecutor.cpp similarity index 100% rename from src/libada/AdaFingerKinematicSimulationPositionCommandExecutor.cpp rename to src/AdaFingerKinematicSimulationPositionCommandExecutor.cpp diff --git a/src/libada/AdaHand.cpp b/src/AdaHand.cpp similarity index 100% rename from src/libada/AdaHand.cpp rename to src/AdaHand.cpp diff --git a/src/libada/AdaHandKinematicSimulationPositionCommandExecutor.cpp b/src/AdaHandKinematicSimulationPositionCommandExecutor.cpp similarity index 100% rename from src/libada/AdaHandKinematicSimulationPositionCommandExecutor.cpp rename to src/AdaHandKinematicSimulationPositionCommandExecutor.cpp diff --git a/src/libada/util.cpp b/src/util.cpp similarity index 100% rename from src/libada/util.cpp rename to src/util.cpp From 26420596bb1721ff9bf4d83c45da5e3f0ff7f48c Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Mon, 15 Mar 2021 17:17:24 -0700 Subject: [PATCH 22/38] Ran simple autopep8 on python scripts --- python/adapy/pr_tsr_py.cpp | 6 ++---- python/tests/simple_trajectories.py | 21 ++++++++++----------- 2 files changed, 12 insertions(+), 15 deletions(-) diff --git a/python/adapy/pr_tsr_py.cpp b/python/adapy/pr_tsr_py.cpp index a5eb181..12c585a 100644 --- a/python/adapy/pr_tsr_py.cpp +++ b/python/adapy/pr_tsr_py.cpp @@ -26,8 +26,7 @@ void Pr_Tsr(pybind11::module& m) "set_T0_w", [](aikido::constraint::dart::TSR* self, Eigen::Matrix4d& pose) -> void { - auto transform = matrixToIsometry(pose); - self->mT0_w = transform; + self->mT0_w = matrixToIsometry(pose); }) .def( "set_Bw", @@ -43,7 +42,6 @@ void Pr_Tsr(pybind11::module& m) "set_Tw_e", [](aikido::constraint::dart::TSR* self, Eigen::Matrix4d& pose) -> void { - auto transform = matrixToIsometry(pose); - self->mTw_e = transform; + self->mTw_e = matrixToIsometry(pose); }); } diff --git a/python/tests/simple_trajectories.py b/python/tests/simple_trajectories.py index b8ed78b..cdb2c71 100755 --- a/python/tests/simple_trajectories.py +++ b/python/tests/simple_trajectories.py @@ -9,11 +9,11 @@ rospy.init_node("adapy_simple_traj") roscpp_init('adapy_simple_traj', []) rate = rospy.Rate(10) -is_sim = False +IS_SIM = False if not rospy.is_shutdown(): - ada = adapy.Ada(is_sim) - if not is_sim: + ada = adapy.Ada(IS_SIM) + if not IS_SIM: ada.start_trajectory_executor() viewer = ada.start_viewer("dart_markers/simple_trajectories", "map") canURDFUri = "package://pr_assets/data/objects/can.urdf" @@ -40,12 +40,11 @@ positions4[1] -= 0.4 positions4[2] += 0.6 - 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)] + 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.compute_joint_space_path(arm_state_space, waypoints) - traj_rev = ada.compute_joint_space_path(arm_state_space, waypoints_rev) + traj_rev = ada.compute_joint_space_path( + arm_state_space, waypoints_rev) print("") print("CONTINUE TO EXECUTE") @@ -57,8 +56,8 @@ print("") print("CLOSING HAND") print("") - preshape = [1.1, 1.1] - ada.get_hand().execute_preshape(preshape) + PRESHAPE = [1.1, 1.1] + ada.get_hand().execute_preshape(PRESHAPE) print("") print("CONTINUE TO EXECUTE REVERSE") @@ -76,5 +75,5 @@ print("DONE! CONTINUE TO EXIT") print("") pdb.set_trace() - if not is_sim: + if not IS_SIM: ada.stop_trajectory_executor() From 5f6b6ad5edab50308f4532d8590e6d049e2fb58e Mon Sep 17 00:00:00 2001 From: Ethan Gordon Date: Mon, 15 Mar 2021 17:23:21 -0700 Subject: [PATCH 23/38] Renamed as per PR comments --- CMakeLists.txt | 10 +++++----- python/adapy/{ada_py.cpp => ada_bindings.cpp} | 0 python/adapy/{aikido_py.cpp => aikido_bindings.cpp} | 0 python/adapy/{dart_py.cpp => dart_bindings.cpp} | 0 python/adapy/{ik_py.cpp => ik_bindings.cpp} | 0 python/adapy/{pr_tsr_py.cpp => pr_tsr_bindings.cpp} | 0 6 files changed, 5 insertions(+), 5 deletions(-) rename python/adapy/{ada_py.cpp => ada_bindings.cpp} (100%) rename python/adapy/{aikido_py.cpp => aikido_bindings.cpp} (100%) rename python/adapy/{dart_py.cpp => dart_bindings.cpp} (100%) rename python/adapy/{ik_py.cpp => ik_bindings.cpp} (100%) rename python/adapy/{pr_tsr_py.cpp => pr_tsr_bindings.cpp} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 047a332..1bde2a6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -150,11 +150,11 @@ if(pybind11_FOUND) pybind11_add_module(adapy MODULE python/adapy/adapy.cpp - python/adapy/ada_py.cpp - python/adapy/aikido_py.cpp - python/adapy/dart_py.cpp - python/adapy/pr_tsr_py.cpp - python/adapy/ik_py.cpp + python/adapy/ada_bindings.cpp + python/adapy/aikido_bindings.cpp + python/adapy/dart_bindings.cpp + python/adapy/pr_tsr_bindings.cpp + python/adapy/ik_bindings.cpp ) target_include_directories(adapy diff --git a/python/adapy/ada_py.cpp b/python/adapy/ada_bindings.cpp similarity index 100% rename from python/adapy/ada_py.cpp rename to python/adapy/ada_bindings.cpp diff --git a/python/adapy/aikido_py.cpp b/python/adapy/aikido_bindings.cpp similarity index 100% rename from python/adapy/aikido_py.cpp rename to python/adapy/aikido_bindings.cpp diff --git a/python/adapy/dart_py.cpp b/python/adapy/dart_bindings.cpp similarity index 100% rename from python/adapy/dart_py.cpp rename to python/adapy/dart_bindings.cpp diff --git a/python/adapy/ik_py.cpp b/python/adapy/ik_bindings.cpp similarity index 100% rename from python/adapy/ik_py.cpp rename to python/adapy/ik_bindings.cpp diff --git a/python/adapy/pr_tsr_py.cpp b/python/adapy/pr_tsr_bindings.cpp similarity index 100% rename from python/adapy/pr_tsr_py.cpp rename to python/adapy/pr_tsr_bindings.cpp From 97892ad2e4c966e370ffc8c8ed6766c72a206b60 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Thu, 25 Mar 2021 23:38:40 -0700 Subject: [PATCH 24/38] Set default in python simple_trajectories to sim. --- python/tests/simple_trajectories.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/tests/simple_trajectories.py b/python/tests/simple_trajectories.py index cdb2c71..1219dce 100755 --- a/python/tests/simple_trajectories.py +++ b/python/tests/simple_trajectories.py @@ -9,7 +9,7 @@ rospy.init_node("adapy_simple_traj") roscpp_init('adapy_simple_traj', []) rate = rospy.Rate(10) -IS_SIM = False +IS_SIM = True if not rospy.is_shutdown(): ada = adapy.Ada(IS_SIM) From ee5f712012a2560da796b9a47992f5375650551b Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 16:41:12 -0700 Subject: [PATCH 25/38] Add python binding cpp files back to format search. --- CMakeLists.txt | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1bde2a6..82e036d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -236,7 +236,13 @@ if(CLANG_FORMAT_EXECUTABLE) file(GLOB_RECURSE ALL_SOURCE_FILES LIST_DIRECTORIES false - include/*.h include/*.hpp src/*.c src/*.cpp tests/*.cpp tests/*.hpp) + include/*.h + include/*.hpp + src/*.c + src/*.cpp + tests/*.cpp + tests/*.hpp + python/*.cpp) clang_format_add_sources(${ALL_SOURCE_FILES}) From 42939c9983786a164612835c8147d1f74d6e7a3c Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 16:43:18 -0700 Subject: [PATCH 26/38] Run make format. --- include/libada/Ada.hpp | 3 ++- include/libada/detail/Ada-impl.hpp | 8 +++++--- python/adapy/aikido_bindings.cpp | 16 ++++++---------- python/adapy/pr_tsr_bindings.cpp | 12 ++++-------- 4 files changed, 17 insertions(+), 22 deletions(-) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 2165294..3ed8cea 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -89,7 +89,8 @@ class Ada final : public aikido::robot::Robot const dart::common::Uri& adaUrdfUri = defaultAdaUrdfUri, const dart::common::Uri& adaSrdfUri = defaultAdaSrdfUri, const std::string& endEffectorName = "j2n6s200_end_effector", - const std::string& armTrajectoryExecutorName = "rewd_trajectory_controller", + const std::string& armTrajectoryExecutorName + = "rewd_trajectory_controller", const ::ros::NodeHandle* node = nullptr, aikido::common::RNG::result_type rngSeed = std::random_device{}(), const dart::common::ResourceRetrieverPtr& retriever diff --git a/include/libada/detail/Ada-impl.hpp b/include/libada/detail/Ada-impl.hpp index 3df7ecf..991aaf4 100644 --- a/include/libada/detail/Ada-impl.hpp +++ b/include/libada/detail/Ada-impl.hpp @@ -17,14 +17,16 @@ aikido::trajectory::UniqueSplinePtr Ada::postProcessPath( bool velLimitsInvalid = (velocityLimits.squaredNorm() == 0.0) || velocityLimits.size() != getVelocityLimits().size(); - auto sentVelocityLimits - = velLimitsInvalid ? DEFAULT_LIMITS_BUFFER * getVelocityLimits() : velocityLimits; + auto sentVelocityLimits = velLimitsInvalid + ? DEFAULT_LIMITS_BUFFER * getVelocityLimits() + : velocityLimits; bool accLimitsInvalid = (accelerationLimits.squaredNorm() == 0.0) || accelerationLimits.size() != getAccelerationLimits().size(); auto sentAccelerationLimits - = accLimitsInvalid ? DEFAULT_LIMITS_BUFFER * getAccelerationLimits() : accelerationLimits; + = accLimitsInvalid ? DEFAULT_LIMITS_BUFFER * getAccelerationLimits() + : accelerationLimits; return mRobot->postProcessPath( sentVelocityLimits, diff --git a/python/adapy/aikido_bindings.cpp b/python/adapy/aikido_bindings.cpp index 2de09a5..587c631 100644 --- a/python/adapy/aikido_bindings.cpp +++ b/python/adapy/aikido_bindings.cpp @@ -50,9 +50,8 @@ void Aikido(pybind11::module& m) -> std::shared_ptr<::dart::dynamics::Skeleton> { auto transform = vectorToIsometry(objectPose); - const auto skeleton - = aikido::io::loadSkeletonFromURDF( - std::make_shared(), uri); + const auto skeleton = aikido::io::loadSkeletonFromURDF( + std::make_shared(), uri); if (!skeleton) throw std::runtime_error("unable to load '" + uri + "'"); @@ -71,9 +70,8 @@ void Aikido(pybind11::module& m) -> std::shared_ptr<::dart::dynamics::Skeleton> { auto transform = matrixToIsometry(objectPose); - const auto skeleton - = aikido::io::loadSkeletonFromURDF( - std::make_shared(), uri); + const auto skeleton = aikido::io::loadSkeletonFromURDF( + std::make_shared(), uri); if (!skeleton) throw std::runtime_error("unable to load '" + uri + "'"); @@ -92,10 +90,8 @@ void Aikido(pybind11::module& m) }) .def( "get_skeleton", - [](aikido::planner::World* self, - int i) -> dart::dynamics::SkeletonPtr { - return self->getSkeleton(i); - }); + [](aikido::planner::World* self, int i) + -> dart::dynamics::SkeletonPtr { return self->getSkeleton(i); }); py::class_< aikido::rviz::InteractiveMarkerViewer, std::shared_ptr>( diff --git a/python/adapy/pr_tsr_bindings.cpp b/python/adapy/pr_tsr_bindings.cpp index 12c585a..6ecf874 100644 --- a/python/adapy/pr_tsr_bindings.cpp +++ b/python/adapy/pr_tsr_bindings.cpp @@ -24,10 +24,8 @@ void Pr_Tsr(pybind11::module& m) }) .def( "set_T0_w", - [](aikido::constraint::dart::TSR* self, - Eigen::Matrix4d& pose) -> void { - self->mT0_w = matrixToIsometry(pose); - }) + [](aikido::constraint::dart::TSR* self, Eigen::Matrix4d& pose) + -> void { self->mT0_w = matrixToIsometry(pose); }) .def( "set_Bw", [](aikido::constraint::dart::TSR* self, Eigen::MatrixXf& Bw) -> void { @@ -40,8 +38,6 @@ void Pr_Tsr(pybind11::module& m) }) .def( "set_Tw_e", - [](aikido::constraint::dart::TSR* self, - Eigen::Matrix4d& pose) -> void { - self->mTw_e = matrixToIsometry(pose); - }); + [](aikido::constraint::dart::TSR* self, Eigen::Matrix4d& pose) + -> void { self->mTw_e = matrixToIsometry(pose); }); } From cd3e5ecbbdb65d8b8b2faa0142ea50a7b155ed3d Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 17:07:55 -0700 Subject: [PATCH 27/38] Start rewriting ada_bindings.cpp without lambdas. --- python/adapy/ada_bindings.cpp | 78 +++++++++++++++++++++-------------- 1 file changed, 48 insertions(+), 30 deletions(-) diff --git a/python/adapy/ada_bindings.cpp b/python/adapy/ada_bindings.cpp index e42ee7a..fda9211 100644 --- a/python/adapy/ada_bindings.cpp +++ b/python/adapy/ada_bindings.cpp @@ -14,43 +14,61 @@ namespace py = pybind11; using aikido::planner::kunzretimer::KunzRetimer; +// NOTE: These functions are bound and define the Python API. + +std::shared_ptr +get_self_collision_constraint(ada::Ada* self) +{ + return self->getSelfCollisionConstraint( + self->getStateSpace(), self->getMetaSkeleton()); +} + +void start_trajectory_executor(ada::Ada* self) +{ + self->startTrajectoryExecutor(); +} + +void stop_trajectory_executor(ada::Ada* self) +{ + self->stopTrajectoryExecutor(); +} + +std::string get_name(ada::Ada* self) +{ + return self->getName(); +} + +aikido::planner::WorldPtr get_world(ada::Ada* self) +{ + return self->getWorld(); +} + +ada::AdaHandPtr get_hand(ada::Ada* self) +{ + return self->getHand(); +} + +dart::dynamics::MetaSkeletonPtr get_skeleton(ada::Ada* self) +{ + return self->getMetaSkeleton(); +} + +//====================================LIBADA==================================== + void Ada(pybind11::module& m) { - //====================================LIBADA========================================================== py::class_>(m, "Ada") .def(py::init([](bool simulation) -> std::shared_ptr { return std::make_shared( aikido::planner::World::create(), simulation); })) - .def( - "get_self_collision_constraint", - [](ada::Ada* self) - -> std::shared_ptr { - return self->getSelfCollisionConstraint( - self->getStateSpace(), self->getMetaSkeleton()); - }) - .def( - "start_trajectory_executor", - [](ada::Ada* self) -> void { self->startTrajectoryExecutor(); }) - .def( - "stop_trajectory_executor", - [](ada::Ada* self) -> void { self->stopTrajectoryExecutor(); }) - .def( - "get_name", - [](ada::Ada* self) -> std::string { return self->getName(); }) - .def( - "get_world", - [](ada::Ada* self) -> aikido::planner::WorldPtr { - return self->getWorld(); - }) - .def( - "get_hand", - [](ada::Ada* self) -> ada::AdaHandPtr { return self->getHand(); }) - .def( - "get_skeleton", - [](ada::Ada* self) -> dart::dynamics::MetaSkeletonPtr { - return self->getMetaSkeleton(); - }) + .def("get_self_collision_constraint", get_self_collision_constraint) + .def("start_trajectory_executor", start_trajectory_executor) + .def("stop_trajectory_executor", stop_trajectory_executor) + .def("get_name", get_name) + .def("get_world", get_world) + .def("get_hand", get_hand) + .def("get_skeleton", get_skeleton) .def( "get_arm_skeleton", [](ada::Ada* self) -> dart::dynamics::MetaSkeletonPtr { From 53e2bb86bc21885fbf86d4e77c99074090a04760 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 17:29:31 -0700 Subject: [PATCH 28/38] Finish rewriting Ada in ada_bindings.cpp without lambdas. --- python/adapy/ada_bindings.cpp | 271 +++++++++++++++++----------------- 1 file changed, 138 insertions(+), 133 deletions(-) diff --git a/python/adapy/ada_bindings.cpp b/python/adapy/ada_bindings.cpp index fda9211..3d2d1ac 100644 --- a/python/adapy/ada_bindings.cpp +++ b/python/adapy/ada_bindings.cpp @@ -14,7 +14,7 @@ namespace py = pybind11; using aikido::planner::kunzretimer::KunzRetimer; -// NOTE: These functions are bound and define the Python API. +// NOTE: These functions define the Python API for Ada. std::shared_ptr get_self_collision_constraint(ada::Ada* self) @@ -53,6 +53,132 @@ dart::dynamics::MetaSkeletonPtr get_skeleton(ada::Ada* self) return self->getMetaSkeleton(); } +dart::dynamics::MetaSkeletonPtr get_arm_skeleton(ada::Ada* self) +{ + return self->getArm()->getMetaSkeleton(); +} + +aikido::statespace::dart::MetaSkeletonStateSpacePtr get_arm_state_space( + ada::Ada* self) +{ + return self->getArm()->getStateSpace(); +} + +void set_positions(ada::Ada* self, const Eigen::VectorXd& configuration) +{ + auto arm = self->getArm(); + auto armSkeleton = arm->getMetaSkeleton(); + armSkeleton->setPositions(configuration); +} + +std::shared_ptr +set_up_collision_detection( + ada::Ada* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& armSpace, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + const std::vector& envSkeletons) +{ + dart::collision::FCLCollisionDetectorPtr collisionDetector + = dart::collision::FCLCollisionDetector::create(); + std::shared_ptr armCollisionGroup + = collisionDetector->createCollisionGroup(self->getMetaSkeleton().get()); + std::shared_ptr envCollisionGroup + = collisionDetector->createCollisionGroup(); + for (const auto& envSkeleton : envSkeletons) + { + envCollisionGroup->addShapeFramesOf(envSkeleton.get()); + } + std::shared_ptr + collisionFreeConstraint + = std::make_shared( + armSpace, armSkeleton, collisionDetector); + collisionFreeConstraint->addPairwiseCheck( + armCollisionGroup, envCollisionGroup); + return collisionFreeConstraint; +} + +aikido::constraint::TestablePtr get_full_collision_constraint( + ada::Ada* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& armSpace, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + const aikido::constraint::dart::CollisionFreePtr& collisionFreeConstraint) +{ + return self->getFullCollisionConstraint( + armSpace, armSkeleton, collisionFreeConstraint); +} + +aikido::trajectory::TrajectoryPtr compute_joint_space_path( + ada::Ada* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, + const std::vector>& waypoints) +{ + return self->computeRetimedJointSpacePath(stateSpace, waypoints); +} + +aikido::trajectory::TrajectoryPtr compute_smooth_joint_space_path( + ada::Ada* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, + const std::vector>& waypoints, + const aikido::constraint::dart::CollisionFreePtr& collisionFreeConstraint + = nullptr) +{ + return self->computeSmoothJointSpacePath( + stateSpace, waypoints, collisionFreeConstraint); +} + +aikido::trajectory::TrajectoryPtr compute_retime_path( + ada::Ada* self, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + aikido::trajectory::InterpolatedPtr trajectory_ptr) +{ + auto satisfied = std::make_shared( + trajectory_ptr->getStateSpace()); + return self->postProcessPath( + trajectory_ptr.get(), + satisfied, + ada::KunzParams(), + armSkeleton->getVelocityUpperLimits(), + armSkeleton->getAccelerationUpperLimits()); +} + +aikido::trajectory::TrajectoryPtr plan_to_configuration( + ada::Ada* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& armSpace, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + const Eigen::VectorXd& configuration) +{ + auto state = armSpace->createState(); + armSpace->convertPositionsToState(configuration, state); + auto trajectory + = self->planToConfiguration(armSpace, armSkeleton, state, nullptr, 10); + return trajectory; +} + +void execute_trajectory( + ada::Ada* self, const aikido::trajectory::TrajectoryPtr& trajectory) +{ + auto future = self->executeTrajectory(trajectory); + + if (!future.valid()) + { + std::__throw_future_error(0); + } + future.wait(); + // Throw any exceptions + future.get(); +} + +std::shared_ptr start_viewer( + ada::Ada* self, + const std::string& topicName, + const std::string& baseFrameName) +{ + auto viewer = std::make_shared( + topicName, baseFrameName, self->getWorld()); + viewer->setAutoUpdate(true); + return viewer; +} + //====================================LIBADA==================================== void Ada(pybind11::module& m) @@ -69,138 +195,17 @@ void Ada(pybind11::module& m) .def("get_world", get_world) .def("get_hand", get_hand) .def("get_skeleton", get_skeleton) - .def( - "get_arm_skeleton", - [](ada::Ada* self) -> dart::dynamics::MetaSkeletonPtr { - return self->getArm()->getMetaSkeleton(); - }) - .def( - "get_arm_state_space", - [](ada::Ada* self) - -> aikido::statespace::dart::MetaSkeletonStateSpacePtr { - return self->getArm()->getStateSpace(); - }) - .def( - "set_positions", - [](ada::Ada* self, const Eigen::VectorXd& configuration) -> void { - auto arm = self->getArm(); - auto armSkeleton = arm->getMetaSkeleton(); - armSkeleton->setPositions(configuration); - }) - .def( - "set_up_collision_detection", - [](ada::Ada* self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& - armSpace, - const dart::dynamics::MetaSkeletonPtr& armSkeleton, - const std::vector& envSkeletons) - -> std::shared_ptr { - dart::collision::FCLCollisionDetectorPtr collisionDetector - = dart::collision::FCLCollisionDetector::create(); - std::shared_ptr armCollisionGroup - = collisionDetector->createCollisionGroup( - self->getMetaSkeleton().get()); - std::shared_ptr envCollisionGroup - = collisionDetector->createCollisionGroup(); - for (const auto& envSkeleton : envSkeletons) - { - envCollisionGroup->addShapeFramesOf(envSkeleton.get()); - } - std::shared_ptr - collisionFreeConstraint - = std::make_shared( - armSpace, armSkeleton, collisionDetector); - collisionFreeConstraint->addPairwiseCheck( - armCollisionGroup, envCollisionGroup); - return collisionFreeConstraint; - }) - .def( - "get_full_collision_constraint", - [](ada::Ada* self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& - armSpace, - const dart::dynamics::MetaSkeletonPtr& armSkeleton, - const aikido::constraint::dart::CollisionFreePtr& - collisionFreeConstraint) -> aikido::constraint::TestablePtr { - return self->getFullCollisionConstraint( - armSpace, armSkeleton, collisionFreeConstraint); - }) - .def( - "compute_joint_space_path", - [](ada::Ada* self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& - stateSpace, - const std::vector>& waypoints) - -> aikido::trajectory::TrajectoryPtr { - return self->computeRetimedJointSpacePath(stateSpace, waypoints); - }) - .def( - "compute_smooth_joint_space_path", - [](ada::Ada* self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& - stateSpace, - const std::vector>& waypoints, - const aikido::constraint::dart::CollisionFreePtr& - collisionFreeConstraint - = nullptr) -> aikido::trajectory::TrajectoryPtr { - return self->computeSmoothJointSpacePath( - stateSpace, waypoints, collisionFreeConstraint); - }) - .def( - "compute_retime_path", - [](ada::Ada* self, - const dart::dynamics::MetaSkeletonPtr& armSkeleton, - aikido::trajectory::InterpolatedPtr trajectory_ptr) - -> aikido::trajectory::TrajectoryPtr { - auto satisfied = std::make_shared( - trajectory_ptr->getStateSpace()); - return self->postProcessPath( - trajectory_ptr.get(), - satisfied, - ada::KunzParams(), - armSkeleton->getVelocityUpperLimits(), - armSkeleton->getAccelerationUpperLimits()); - }) - .def( - "plan_to_configuration", - [](ada::Ada* self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& - armSpace, - const dart::dynamics::MetaSkeletonPtr& armSkeleton, - const Eigen::VectorXd& configuration) - -> aikido::trajectory::TrajectoryPtr { - auto state = armSpace->createState(); - armSpace->convertPositionsToState(configuration, state); - auto trajectory = self->planToConfiguration( - armSpace, armSkeleton, state, nullptr, 10); - return trajectory; - }) - .def( - "execute_trajectory", - [](ada::Ada* self, - const aikido::trajectory::TrajectoryPtr& trajectory) -> void { - auto future = self->executeTrajectory(trajectory); - - if (!future.valid()) - { - std::__throw_future_error(0); - } - future.wait(); - // Throw any exceptions - future.get(); - }) - .def( - "start_viewer", - [](ada::Ada* self, - const std::string& topicName, - const std::string& baseFrameName) - -> std::shared_ptr { - auto viewer - = std::make_shared( - topicName, baseFrameName, self->getWorld()); - viewer->setAutoUpdate(true); - return viewer; - }); + .def("get_arm_skeleton", get_arm_skeleton) + .def("get_arm_state_space", get_arm_state_space) + .def("set_positions", set_positions) + .def("set_up_collision_detection", set_up_collision_detection) + .def("get_full_collision_constraint", get_full_collision_constraint) + .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("execute_trajectory", execute_trajectory) + .def("start_viewer", start_viewer); py::class_>(m, "AdaHand") .def( "get_skeleton", From 2697e19254fa7bd897fbfd18862316e6039ad30c Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 17:51:36 -0700 Subject: [PATCH 29/38] Finish re-writing ada_bindings.cpp without lambdas. --- python/adapy/ada_bindings.cpp | 158 ++++++++++++++++++---------------- 1 file changed, 85 insertions(+), 73 deletions(-) diff --git a/python/adapy/ada_bindings.cpp b/python/adapy/ada_bindings.cpp index 3d2d1ac..1c405d9 100644 --- a/python/adapy/ada_bindings.cpp +++ b/python/adapy/ada_bindings.cpp @@ -14,6 +14,7 @@ namespace py = pybind11; using aikido::planner::kunzretimer::KunzRetimer; +//============================================================================== // NOTE: These functions define the Python API for Ada. std::shared_ptr @@ -179,6 +180,82 @@ std::shared_ptr start_viewer( return viewer; } +//============================================================================== +// NOTE: These functions define the Python API for AdaHand. + +dart::dynamics::MetaSkeletonPtr hand_get_skeleton(ada::AdaHand* self) +{ + return self->getMetaSkeleton(); +} + +aikido::statespace::dart::MetaSkeletonStateSpacePtr hand_get_state_space( + ada::AdaHand* self) +{ + auto handSkeleton = self->getMetaSkeleton(); + auto handSpace + = std::make_shared( + handSkeleton.get()); + return handSpace; +} + +void execute_preshape(ada::AdaHand* self, const Eigen::Vector2d& d) +{ + auto future = self->executePreshape(d); + + if (!future.valid()) + { + std::__throw_future_error(0); + } + + future.wait(); + // Throw any exceptions + future.get(); +} + +void hand_open(ada::AdaHand* self) +{ + auto future = self->executePreshape("open"); + + if (!future.valid()) + { + std::__throw_future_error(0); + } + + future.wait(); + // Throw any exceptions + future.get(); +} + +void hand_close(ada::AdaHand* self) +{ + auto future = self->executePreshape("closed"); + + if (!future.valid()) + { + std::__throw_future_error(0); + } + + future.wait(); + // Throw any exceptions + future.get(); +} + +Eigen::Matrix4d get_endeffector_transform( + ada::AdaHand* self, const std::string& objectType) +{ + return self->getEndEffectorTransform(objectType)->matrix(); +} + +dart::dynamics::BodyNode* get_endeffector_body_node(ada::AdaHand* self) +{ + return self->getEndEffectorBodyNode(); +} + +void grab(ada::AdaHand* self, dart::dynamics::SkeletonPtr object) +{ + self->grab(object); +} + //====================================LIBADA==================================== void Ada(pybind11::module& m) @@ -207,77 +284,12 @@ void Ada(pybind11::module& m) .def("execute_trajectory", execute_trajectory) .def("start_viewer", start_viewer); py::class_>(m, "AdaHand") - .def( - "get_skeleton", - [](ada::AdaHand* self) -> dart::dynamics::MetaSkeletonPtr { - return self->getMetaSkeleton(); - }) - .def( - "get_state_space", - [](ada::AdaHand* self) - -> aikido::statespace::dart::MetaSkeletonStateSpacePtr { - auto handSkeleton = self->getMetaSkeleton(); - auto handSpace = std::make_shared< - aikido::statespace::dart::MetaSkeletonStateSpace>( - handSkeleton.get()); - return handSpace; - }) - .def( - "execute_preshape", - [](ada::AdaHand* self, const Eigen::Vector2d& d) -> void { - auto future = self->executePreshape(d); - - if (!future.valid()) - { - std::__throw_future_error(0); - } - - future.wait(); - // Throw any exceptions - future.get(); - }) - .def( - "open", - [](ada::AdaHand* self) -> void { - auto future = self->executePreshape("open"); - - if (!future.valid()) - { - std::__throw_future_error(0); - } - - future.wait(); - // Throw any exceptions - future.get(); - }) - .def( - "close", - [](ada::AdaHand* self) -> void { - auto future = self->executePreshape("closed"); - - if (!future.valid()) - { - std::__throw_future_error(0); - } - - future.wait(); - // Throw any exceptions - future.get(); - }) - .def( - "get_endeffector_transform", - [](ada::AdaHand* self, - const std::string& objectType) -> Eigen::Matrix4d { - return self->getEndEffectorTransform(objectType)->matrix(); - }) - .def( - "get_endeffector_body_node", - [](ada::AdaHand* self) -> dart::dynamics::BodyNode* { - return self->getEndEffectorBodyNode(); - }) - .def( - "grab", - [](ada::AdaHand* self, dart::dynamics::SkeletonPtr object) -> void { - self->grab(object); - }); + .def("get_skeleton", hand_get_skeleton) + .def("get_state_space", hand_get_state_space) + .def("execute_preshape", execute_preshape) + .def("open", hand_open) + .def("close", hand_close) + .def("get_endeffector_transform", get_endeffector_transform) + .def("get_endeffector_body_node", get_endeffector_body_node) + .def("grab", grab); } From 6c5e7fd43b294a41f318696ccd172dc20cf7321d Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 19:44:18 -0700 Subject: [PATCH 30/38] Rewrite World bindings without lambdas. --- python/adapy/aikido_bindings.cpp | 102 +++++++++++++++++-------------- 1 file changed, 56 insertions(+), 46 deletions(-) diff --git a/python/adapy/aikido_bindings.cpp b/python/adapy/aikido_bindings.cpp index 587c631..177734f 100644 --- a/python/adapy/aikido_bindings.cpp +++ b/python/adapy/aikido_bindings.cpp @@ -37,61 +37,71 @@ Eigen::Isometry3d matrixToIsometry(Eigen::Matrix4d& poseMatrix) return pose; } -void Aikido(pybind11::module& m) +//============================================================================== +// NOTE: These functions define the Python API for World. + +std::shared_ptr<::dart::dynamics::Skeleton> add_body_from_urdf( + aikido::planner::World* self, + const std::string& uri, + std::vector objectPose) { - //====================================AIKIDO============================================================================= - py::class_>( - m, "World") - .def( - "add_body_from_urdf", - [](aikido::planner::World* self, - const std::string& uri, - std::vector objectPose) - -> std::shared_ptr<::dart::dynamics::Skeleton> { - auto transform = vectorToIsometry(objectPose); + auto transform = vectorToIsometry(objectPose); - const auto skeleton = aikido::io::loadSkeletonFromURDF( - std::make_shared(), uri); + const auto skeleton = aikido::io::loadSkeletonFromURDF( + std::make_shared(), uri); - if (!skeleton) - throw std::runtime_error("unable to load '" + uri + "'"); + if (!skeleton) + throw std::runtime_error("unable to load '" + uri + "'"); - dynamic_cast(skeleton->getJoint(0)) - ->setTransform(transform); + dynamic_cast(skeleton->getJoint(0)) + ->setTransform(transform); - self->addSkeleton(skeleton); - return skeleton; - }) - .def( - "add_body_from_urdf_matrix", - [](aikido::planner::World* self, - const std::string& uri, - Eigen::Matrix4d& objectPose) - -> std::shared_ptr<::dart::dynamics::Skeleton> { - auto transform = matrixToIsometry(objectPose); + self->addSkeleton(skeleton); + return skeleton; +} - const auto skeleton = aikido::io::loadSkeletonFromURDF( - std::make_shared(), uri); +std::shared_ptr<::dart::dynamics::Skeleton> add_body_from_urdf_matrix( + aikido::planner::World* self, + const std::string& uri, + Eigen::Matrix4d& objectPose) +{ + auto transform = matrixToIsometry(objectPose); - if (!skeleton) - throw std::runtime_error("unable to load '" + uri + "'"); + const auto skeleton = aikido::io::loadSkeletonFromURDF( + std::make_shared(), uri); - dynamic_cast(skeleton->getJoint(0)) - ->setTransform(transform); + if (!skeleton) + throw std::runtime_error("unable to load '" + uri + "'"); - self->addSkeleton(skeleton); - return skeleton; - }) - .def( - "remove_skeleton", - [](aikido::planner::World* self, - std::shared_ptr<::dart::dynamics::Skeleton> skeleton) -> void { - self->removeSkeleton(skeleton); - }) - .def( - "get_skeleton", - [](aikido::planner::World* self, int i) - -> dart::dynamics::SkeletonPtr { return self->getSkeleton(i); }); + dynamic_cast(skeleton->getJoint(0)) + ->setTransform(transform); + + self->addSkeleton(skeleton); + return skeleton; +} + +void remove_skeleton( + aikido::planner::World* self, + std::shared_ptr<::dart::dynamics::Skeleton> skeleton) +{ + self->removeSkeleton(skeleton); +} + +dart::dynamics::SkeletonPtr get_skeleton(aikido::planner::World* self, int i) +{ + return self->getSkeleton(i); +} + +//====================================AIKIDO==================================== + +void Aikido(pybind11::module& m) +{ + py::class_>( + m, "World") + .def("add_body_from_urdf", add_body_from_urdf) + .def("add_body_from_urdf_matrix", add_body_from_urdf_matrix) + .def("remove_skeleton", remove_skeleton) + .def("get_skeleton", get_skeleton); py::class_< aikido::rviz::InteractiveMarkerViewer, std::shared_ptr>( From 308badeb95cfb02a160684a87e7621d95e4d8249 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 20:05:35 -0700 Subject: [PATCH 31/38] Finish re-writing aikido_bindings.cpp without lambdas. --- python/adapy/aikido_bindings.cpp | 81 ++++++++++++++++++-------------- 1 file changed, 45 insertions(+), 36 deletions(-) diff --git a/python/adapy/aikido_bindings.cpp b/python/adapy/aikido_bindings.cpp index 177734f..a965f33 100644 --- a/python/adapy/aikido_bindings.cpp +++ b/python/adapy/aikido_bindings.cpp @@ -92,6 +92,47 @@ dart::dynamics::SkeletonPtr get_skeleton(aikido::planner::World* self, int i) return self->getSkeleton(i); } +//============================================================================== +// NOTE: These functions define the Python API for InteractiveMarkerViewer. + +void update(aikido::rviz::InteractiveMarkerViewer* self) +{ + self->update(); +} + +void add_frame( + aikido::rviz::InteractiveMarkerViewer* self, dart::dynamics::BodyNode* node) +{ + self->addFrameMarker(node); +} + +aikido::rviz::TSRMarkerPtr add_tsr_marker( + aikido::rviz::InteractiveMarkerViewer* self, + std::shared_ptr tsr) +{ + return self->addTSRMarker(*tsr.get()); +} + +//============================================================================== +// NOTE: These functions define the Python API for Testable. + +bool is_satisfied( + aikido::constraint::Testable* self, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& armSpace, + const dart::dynamics::MetaSkeletonPtr& armSkeleton, + const Eigen::VectorXd& positions) +{ + auto armState = armSpace->createState(); + armSpace->convertPositionsToState(positions, armState); + auto currentState + = armSpace->getScopedStateFromMetaSkeleton(armSkeleton.get()); + aikido::constraint::DefaultTestableOutcome fullCollisionCheckOutcome; + bool collisionResult + = self->isSatisfied(armState, &fullCollisionCheckOutcome); + armSpace->setState(armSkeleton.get(), currentState); + return collisionResult; +} + //====================================AIKIDO==================================== void Aikido(pybind11::module& m) @@ -106,24 +147,9 @@ void Aikido(pybind11::module& m) aikido::rviz::InteractiveMarkerViewer, std::shared_ptr>( m, "InteractiveMarkerViewer") - .def( - "update", - [](aikido::rviz::InteractiveMarkerViewer* self) -> void { - self->update(); - }) - .def( - "add_frame", - [](aikido::rviz::InteractiveMarkerViewer* self, - dart::dynamics::BodyNode* node) -> void { - self->addFrameMarker(node); - }) - .def( - "add_tsr_marker", - [](aikido::rviz::InteractiveMarkerViewer* self, - std::shared_ptr tsr) - -> aikido::rviz::TSRMarkerPtr { - return self->addTSRMarker(*tsr.get()); - }); + .def("update", update) + .def("add_frame", add_frame) + .def("add_tsr_marker", add_tsr_marker); py::class_< aikido::constraint::dart::CollisionFree, std::shared_ptr>( @@ -138,22 +164,5 @@ void Aikido(pybind11::module& m) m, "TSRMarker"); py::class_( m, "FullCollisionFree") - .def( - "is_satisfied", - [](aikido::constraint::Testable* self, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& - armSpace, - const dart::dynamics::MetaSkeletonPtr& armSkeleton, - const Eigen::VectorXd& positions) -> bool { - auto armState = armSpace->createState(); - armSpace->convertPositionsToState(positions, armState); - auto currentState - = armSpace->getScopedStateFromMetaSkeleton(armSkeleton.get()); - aikido::constraint::DefaultTestableOutcome - fullCollisionCheckOutcome; - bool collisionResult - = self->isSatisfied(armState, &fullCollisionCheckOutcome); - armSpace->setState(armSkeleton.get(), currentState); - return collisionResult; - }); + .def("is_satisfied", is_satisfied); } From 24f545fa2938dc94ac3a2f1a3e4275f4a10595c6 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 20:35:19 -0700 Subject: [PATCH 32/38] Un-lambda SKeleton bindings. --- python/adapy/dart_bindings.cpp | 38 ++++++++++++++++++++-------------- 1 file changed, 23 insertions(+), 15 deletions(-) diff --git a/python/adapy/dart_bindings.cpp b/python/adapy/dart_bindings.cpp index 8359ef5..84253be 100644 --- a/python/adapy/dart_bindings.cpp +++ b/python/adapy/dart_bindings.cpp @@ -9,26 +9,34 @@ namespace py = pybind11; +//============================================================================== +// NOTE: These functions define the Python API for Skeleton. + +std::string get_name(dart::dynamics::Skeleton* self) +{ + return self->getName(); +} + +Eigen::VectorXd get_positions(dart::dynamics::Skeleton* self) +{ + return self->getPositions(); +} + +int get_num_joints(dart::dynamics::Skeleton* self) +{ + return self->getNumJoints(); +} + +//==================================DART======================================== + void Dart(pybind11::module& m) { - //===================================================DART====================================================================== py::class_< dart::dynamics::Skeleton, std::shared_ptr>(m, "Skeleton") - .def( - "get_name", - [](dart::dynamics::Skeleton* self) -> std::string { - return self->getName(); - }) - .def( - "get_positions", - [](dart::dynamics::Skeleton* self) -> Eigen::VectorXd { - return self->getPositions(); - }) - .def("get_num_joints", [](dart::dynamics::Skeleton* self) -> int { - return self->getNumJoints(); - }); - + .def("get_name", get_name) + .def("get_positions", get_positions) + .def("get_num_joints", get_num_joints); py::class_< dart::dynamics::MetaSkeleton, std::shared_ptr>(m, "MetaSkeleton") From 6a300e0f66d38cab06af90bcb09a2e6bc72ec5fb Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 21:09:40 -0700 Subject: [PATCH 33/38] Finish rewriting dart_bindings.cpp to remove lambdas. --- python/adapy/dart_bindings.cpp | 75 +++++++++++++++++++--------------- 1 file changed, 41 insertions(+), 34 deletions(-) diff --git a/python/adapy/dart_bindings.cpp b/python/adapy/dart_bindings.cpp index 84253be..f15f710 100644 --- a/python/adapy/dart_bindings.cpp +++ b/python/adapy/dart_bindings.cpp @@ -12,21 +12,51 @@ namespace py = pybind11; //============================================================================== // NOTE: These functions define the Python API for Skeleton. -std::string get_name(dart::dynamics::Skeleton* self) +std::string skeleton_get_name(dart::dynamics::Skeleton* self) { return self->getName(); } -Eigen::VectorXd get_positions(dart::dynamics::Skeleton* self) +Eigen::VectorXd skeleton_get_positions(dart::dynamics::Skeleton* self) { return self->getPositions(); } -int get_num_joints(dart::dynamics::Skeleton* self) +int skeleton_get_num_joints(dart::dynamics::Skeleton* self) { return self->getNumJoints(); } +//============================================================================== +// NOTE: These functions define the Python API for MetaSkeleton. + +std::string metaskeleton_get_name(dart::dynamics::MetaSkeleton* self) +{ + return self->getName(); +} + +Eigen::VectorXd metaskeleton_get_positions(dart::dynamics::MetaSkeleton* self) +{ + return self->getPositions(); +} + +int metaskeleton_get_num_joints(dart::dynamics::MetaSkeleton* self) +{ + return self->getNumJoints(); +} + +dart::math::LinearJacobian get_linear_jacobian( + dart::dynamics::MetaSkeleton* self, const dart::dynamics::BodyNode* _node) +{ + return self->getLinearJacobian(_node); +} + +dart::math::Jacobian get_jacobian( + dart::dynamics::MetaSkeleton* self, const dart::dynamics::BodyNode* _node) +{ + return self->getJacobian(_node); +} + //==================================DART======================================== void Dart(pybind11::module& m) @@ -34,38 +64,15 @@ void Dart(pybind11::module& m) py::class_< dart::dynamics::Skeleton, std::shared_ptr>(m, "Skeleton") - .def("get_name", get_name) - .def("get_positions", get_positions) - .def("get_num_joints", get_num_joints); + .def("get_name", skeleton_get_name) + .def("get_positions", skeleton_get_positions) + .def("get_num_joints", skeleton_get_num_joints); py::class_< dart::dynamics::MetaSkeleton, std::shared_ptr>(m, "MetaSkeleton") - .def( - "get_name", - [](dart::dynamics::MetaSkeleton* self) -> std::string { - return self->getName(); - }) - .def( - "get_positions", - [](dart::dynamics::MetaSkeleton* self) -> Eigen::VectorXd { - return self->getPositions(); - }) - .def( - "get_num_joints", - [](dart::dynamics::MetaSkeleton* self) -> int { - return self->getNumJoints(); - }) - .def( - "get_linear_jacobian", - [](dart::dynamics::MetaSkeleton* self, - const dart::dynamics::BodyNode* _node) - -> dart::math::LinearJacobian { - return self->getLinearJacobian(_node); - }) - .def( - "get_jacobian", - [](dart::dynamics::MetaSkeleton* self, - const dart::dynamics::BodyNode* _node) -> dart::math::Jacobian { - return self->getJacobian(_node); - }); + .def("get_name", metaskeleton_get_name) + .def("get_positions", metaskeleton_get_positions) + .def("get_num_joints", metaskeleton_get_num_joints) + .def("get_linear_jacobian", get_linear_jacobian) + .def("get_jacobian", get_jacobian); } From 606cc295e87c82fab7326149056a6dad85993246 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 21:44:31 -0700 Subject: [PATCH 34/38] Finish rewriting ik_bindings.cpp without lambdas. --- python/adapy/ik_bindings.cpp | 107 +++++++++++++++++++---------------- 1 file changed, 59 insertions(+), 48 deletions(-) diff --git a/python/adapy/ik_bindings.cpp b/python/adapy/ik_bindings.cpp index ea95603..10e5623 100644 --- a/python/adapy/ik_bindings.cpp +++ b/python/adapy/ik_bindings.cpp @@ -11,63 +11,74 @@ namespace py = pybind11; +aikido::constraint::dart::InverseKinematicsSampleable create_ik( + dart::dynamics::MetaSkeletonPtr armSkeleton, + aikido::statespace::dart::MetaSkeletonStateSpacePtr armSpace, + aikido::constraint::dart::TSR objectTSR, + dart::dynamics::BodyNode* body_node) +{ + auto goalTSR = std::make_shared(objectTSR); + auto ik = dart::dynamics::InverseKinematics::create(body_node); + ik->setDofs(armSkeleton->getDofs()); + auto rng = std::unique_ptr( + new aikido::common::RNGWrapper(0)); + aikido::constraint::dart::InverseKinematicsSampleable ikSampleable( + armSpace, + armSkeleton, + goalTSR, + aikido::constraint::dart::createSampleableBounds( + armSpace, std::move(rng)), + ik, + 10); + return ikSampleable; +} + +//============================================================================== +// NOTE: These functions define the Python API for IKSampleable. + +std::unique_ptr create_sample_generator( + aikido::constraint::dart::InverseKinematicsSampleable* self) +{ + return self->createSampleGenerator(); +} + +//============================================================================== +// NOTE: These functions define the Python API for IKSampleGenerator. + +bool can_sample(aikido::constraint::SampleGenerator* self) +{ + return self->canSample(); +} + +Eigen::VectorXd sample( + aikido::constraint::SampleGenerator* self, + aikido::statespace::dart::MetaSkeletonStateSpacePtr armSpace) +{ + auto goalState = armSpace->createState(); + bool sampled = self->sample(goalState); + // Init to length-0 vector + Eigen::VectorXd positions; + if (!sampled) + return positions; + armSpace->convertStateToPositions(goalState, positions); + return positions; +} + +//==================================IK========================================== + void IK(pybind11::module& m) { - m.def( - "create_ik", - [](dart::dynamics::MetaSkeletonPtr armSkeleton, - aikido::statespace::dart::MetaSkeletonStateSpacePtr armSpace, - aikido::constraint::dart::TSR objectTSR, - dart::dynamics::BodyNode* body_node) - -> aikido::constraint::dart::InverseKinematicsSampleable { - auto goalTSR - = std::make_shared(objectTSR); - auto ik = dart::dynamics::InverseKinematics::create(body_node); - ik->setDofs(armSkeleton->getDofs()); - auto rng = std::unique_ptr( - new aikido::common::RNGWrapper(0)); - aikido::constraint::dart::InverseKinematicsSampleable ikSampleable( - armSpace, - armSkeleton, - goalTSR, - aikido::constraint::dart::createSampleableBounds( - armSpace, std::move(rng)), - ik, - 10); - return ikSampleable; - }); + m.def("create_ik", create_ik); py::class_< aikido::constraint::dart::InverseKinematicsSampleable, std::shared_ptr>( m, "IKSampleable") - .def( - "create_sample_generator", - [](aikido::constraint::dart::InverseKinematicsSampleable* self) - -> std::unique_ptr { - return self->createSampleGenerator(); - }); + .def("create_sample_generator", create_sample_generator); py::class_(m, "IKSampleGenerator") - .def( - "can_sample", - [](aikido::constraint::SampleGenerator* self) -> bool { - return self->canSample(); - }) - .def( - "sample", - [](aikido::constraint::SampleGenerator* self, - aikido::statespace::dart::MetaSkeletonStateSpacePtr armSpace) - -> Eigen::VectorXd { - auto goalState = armSpace->createState(); - bool sampled = self->sample(goalState); - // Init to length-0 vector - Eigen::VectorXd positions; - if (!sampled) - return positions; - armSpace->convertStateToPositions(goalState, positions); - return positions; - }); + .def("can_sample", can_sample) + .def("sample", sample); py::class_< dart::dynamics::BodyNode, From b689e47ec0c35a20150919cd242a00b1f46061e5 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 21:54:17 -0700 Subject: [PATCH 35/38] Finish rewriting TSR bindings without lambdas. --- python/adapy/pr_tsr_bindings.cpp | 58 +++++++++++++++++++------------- 1 file changed, 35 insertions(+), 23 deletions(-) diff --git a/python/adapy/pr_tsr_bindings.cpp b/python/adapy/pr_tsr_bindings.cpp index 6ecf874..e47604a 100644 --- a/python/adapy/pr_tsr_bindings.cpp +++ b/python/adapy/pr_tsr_bindings.cpp @@ -10,6 +10,36 @@ namespace py = pybind11; Eigen::Isometry3d vectorToIsometry(std::vector& poseVector); Eigen::Isometry3d matrixToIsometry(Eigen::Matrix4d& poseMatrix); +//============================================================================== +// NOTE: These functions define the Python API for TSR. + +Eigen::Matrix4d get_T0_w(aikido::constraint::dart::TSR* self) +{ + return self->mT0_w.matrix(); +} + +void set_T0_w(aikido::constraint::dart::TSR* self, Eigen::Matrix4d& pose) +{ + self->mT0_w = matrixToIsometry(pose); +} + +void set_Bw(aikido::constraint::dart::TSR* self, Eigen::MatrixXf& Bw) +{ + self->mBw = Bw.cast(); +} + +Eigen::Matrix4d get_Tw_e(aikido::constraint::dart::TSR* self) +{ + return self->mTw_e.matrix(); +} + +void set_Tw_e(aikido::constraint::dart::TSR* self, Eigen::Matrix4d& pose) +{ + self->mTw_e = matrixToIsometry(pose); +} + +//==================================TSR========================================= + void Pr_Tsr(pybind11::module& m) { m.def("get_default_TSR", &pr_tsr::getDefaultCanTSR); @@ -17,27 +47,9 @@ void Pr_Tsr(pybind11::module& m) py::class_< aikido::constraint::dart::TSR, std::shared_ptr>(m, "TSR") - .def( - "get_T0_w", - [](aikido::constraint::dart::TSR* self) -> Eigen::Matrix4d { - return self->mT0_w.matrix(); - }) - .def( - "set_T0_w", - [](aikido::constraint::dart::TSR* self, Eigen::Matrix4d& pose) - -> void { self->mT0_w = matrixToIsometry(pose); }) - .def( - "set_Bw", - [](aikido::constraint::dart::TSR* self, Eigen::MatrixXf& Bw) -> void { - self->mBw = Bw.cast(); - }) - .def( - "get_Tw_e", - [](aikido::constraint::dart::TSR* self) -> Eigen::Matrix4d { - return self->mTw_e.matrix(); - }) - .def( - "set_Tw_e", - [](aikido::constraint::dart::TSR* self, Eigen::Matrix4d& pose) - -> void { self->mTw_e = matrixToIsometry(pose); }); + .def("get_T0_w", get_T0_w) + .def("set_T0_w", set_T0_w) + .def("set_Bw", set_Bw) + .def("get_Tw_e", get_Tw_e) + .def("set_Tw_e", set_Tw_e); } From e0cbb309326a77da00162c9aa61697c41604f525 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 21:59:07 -0700 Subject: [PATCH 36/38] Spacing in python bindings. --- python/adapy/ada_bindings.cpp | 1 + python/adapy/aikido_bindings.cpp | 6 ++++++ python/adapy/dart_bindings.cpp | 1 + 3 files changed, 8 insertions(+) diff --git a/python/adapy/ada_bindings.cpp b/python/adapy/ada_bindings.cpp index 1c405d9..0aee723 100644 --- a/python/adapy/ada_bindings.cpp +++ b/python/adapy/ada_bindings.cpp @@ -283,6 +283,7 @@ void Ada(pybind11::module& m) .def("plan_to_configuration", plan_to_configuration) .def("execute_trajectory", execute_trajectory) .def("start_viewer", start_viewer); + py::class_>(m, "AdaHand") .def("get_skeleton", hand_get_skeleton) .def("get_state_space", hand_get_state_space) diff --git a/python/adapy/aikido_bindings.cpp b/python/adapy/aikido_bindings.cpp index a965f33..b9930c0 100644 --- a/python/adapy/aikido_bindings.cpp +++ b/python/adapy/aikido_bindings.cpp @@ -143,6 +143,7 @@ void Aikido(pybind11::module& m) .def("add_body_from_urdf_matrix", add_body_from_urdf_matrix) .def("remove_skeleton", remove_skeleton) .def("get_skeleton", get_skeleton); + py::class_< aikido::rviz::InteractiveMarkerViewer, std::shared_ptr>( @@ -150,18 +151,23 @@ void Aikido(pybind11::module& m) .def("update", update) .def("add_frame", add_frame) .def("add_tsr_marker", add_tsr_marker); + py::class_< aikido::constraint::dart::CollisionFree, std::shared_ptr>( m, "CollisionFree"); + py::class_< aikido::statespace::dart::MetaSkeletonStateSpace, aikido::statespace::dart::MetaSkeletonStateSpacePtr>( m, "MetaSkeletonStateSpace"); + py::class_( m, "Trajectory"); + py::class_( m, "TSRMarker"); + py::class_( m, "FullCollisionFree") .def("is_satisfied", is_satisfied); diff --git a/python/adapy/dart_bindings.cpp b/python/adapy/dart_bindings.cpp index f15f710..f1efc80 100644 --- a/python/adapy/dart_bindings.cpp +++ b/python/adapy/dart_bindings.cpp @@ -67,6 +67,7 @@ void Dart(pybind11::module& m) .def("get_name", skeleton_get_name) .def("get_positions", skeleton_get_positions) .def("get_num_joints", skeleton_get_num_joints); + py::class_< dart::dynamics::MetaSkeleton, std::shared_ptr>(m, "MetaSkeleton") From 76936fe8189dac6c5b021d74423ebe3226879608 Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 22:17:59 -0700 Subject: [PATCH 37/38] Add comments for new cpp methods. --- include/libada/Ada.hpp | 13 +++++++++++++ include/libada/AdaHand.hpp | 2 ++ 2 files changed, 15 insertions(+) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 3ed8cea..55b79cb 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -177,10 +177,23 @@ class Ada final : public aikido::robot::Robot // Runs step with current time. void update(); + /// Generates a timed (but not smoothed) trajectory from the given list of + /// configurations. + /// \param[in] stateSpace The statespace for the trajectory. + /// \param[in] waypoints Ordered configurations to add to the trajectory, each + /// of which is paired with its desired time along the output trajectory. + /// \return Timed trajectory. aikido::trajectory::TrajectoryPtr computeRetimedJointSpacePath( const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, const std::vector>& waypoints); + /// Generates a timed *and* smoothed trajectory from the given list of + /// configurations. + /// \param[in] space The statespace for the trajectory. + /// \param[in] waypoints Ordered configurations to add to the trajectory, each + /// of which is paired with its desired time along the output trajectory. + /// \param[in] collisionFree Collision constraint during post-processing. + /// \return Timed amd smoothed trajectory. aikido::trajectory::TrajectoryPtr computeSmoothJointSpacePath( const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, const std::vector>& waypoints, diff --git a/include/libada/AdaHand.hpp b/include/libada/AdaHand.hpp index 26bb686..145c56f 100644 --- a/include/libada/AdaHand.hpp +++ b/include/libada/AdaHand.hpp @@ -83,6 +83,8 @@ class AdaHand : public aikido::robot::Hand // Documentation inherited. std::future executePreshape(const std::string& preshapeName) override; + /// Sets the hand to the given preshape configuration. + /// \param[in] preshape Configuration of the hand. std::future executePreshape(const Eigen::Vector2d& preshape); // Documentation inherited. From 42c8e36fd103f3c0b11f2274ce8d3f6b3d67fd3d Mon Sep 17 00:00:00 2001 From: sniyaz Date: Sat, 27 Mar 2021 22:30:41 -0700 Subject: [PATCH 38/38] Fix test_collisions.py. --- python/tests/test_collisions.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/python/tests/test_collisions.py b/python/tests/test_collisions.py index 4aade1a..57c74f7 100755 --- a/python/tests/test_collisions.py +++ b/python/tests/test_collisions.py @@ -3,10 +3,13 @@ import adapy import rospy +from moveit_ros_planning_interface._moveit_roscpp_initializer import roscpp_init + # import pdb # pdb.set_trace() rospy.init_node("adapy_simple_traj") +roscpp_init('adapy_simple_traj', []) rate = rospy.Rate(10) ada = adapy.Ada(True)