From 27b13f6e16f55fb5421804a8e2c631922c12148b Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Mon, 17 Aug 2020 13:21:35 +0300 Subject: [PATCH] [robot]: add limits and reset, minor fix in python --- src/examples/dof_maps.cpp | 4 +- src/python/robot.cpp | 64 ++++++++++++- src/robot_dart/robot.cpp | 194 +++++++++++++++++++++++++++++++++++++- src/robot_dart/robot.hpp | 32 ++++++- 4 files changed, 279 insertions(+), 15 deletions(-) diff --git a/src/examples/dof_maps.cpp b/src/examples/dof_maps.cpp index b2c87d2c..1b3e9fcc 100644 --- a/src/examples/dof_maps.cpp +++ b/src/examples/dof_maps.cpp @@ -13,8 +13,8 @@ int main() { std::srand(std::time(NULL)); - std::vector> packages = {{"iiwa14", "iiwa/meshes"}}; - auto global_robot = std::make_shared("iiwa/iiwa14.urdf", packages); + std::vector> packages = {{"iiwa_description", "iiwa/iiwa_description"}}; + auto global_robot = std::make_shared("iiwa/iiwa.urdf", packages); global_robot->fix_to_world(); global_robot->set_position_enforced(true); diff --git a/src/python/robot.cpp b/src/python/robot.cpp index 7eebe95d..93e396e6 100644 --- a/src/python/robot.cpp +++ b/src/python/robot.cpp @@ -14,9 +14,21 @@ namespace robot_dart { using namespace robot_dart; // Robot class py::class_>(m, "Robot") - .def(py::init>&, const std::string&, bool>()) - .def(py::init()) - .def(py::init()) + .def(py::init>&, const std::string&, bool, bool>(), + py::arg("model_file"), + py::arg("packages"), + py::arg("robot_name") = "robot", + py::arg("is_urdf_string") = false, + py::arg("cast_shadows") = true) + .def(py::init(), + py::arg("model_file"), + py::arg("robot_name") = "robot", + py::arg("is_urdf_string") = false, + py::arg("cast_shadows") = true) + .def(py::init(), + py::arg("skeleton"), + py::arg("robot_name") = "robot", + py::arg("cast_shadows") = true) .def("clone", &Robot::clone) .def("clone_ghost", &Robot::clone_ghost, @@ -48,6 +60,8 @@ namespace robot_dart { .def("fixed", &Robot::fixed) .def("free", &Robot::free) + .def("reset", &Robot::reset) + .def("set_actuator_types", &Robot::set_actuator_types, py::arg("type"), py::arg("joint_names") = std::vector(), @@ -111,24 +125,68 @@ namespace robot_dart { py::arg("positions"), py::arg("dof_names") = std::vector()) + .def("position_lower_limits", &Robot::position_lower_limits, + py::arg("dof_names") = std::vector()) + .def("set_position_lower_limits", &Robot::set_position_lower_limits, + py::arg("positions"), + py::arg("dof_names") = std::vector()) + .def("position_upper_limits", &Robot::position_upper_limits, + py::arg("dof_names") = std::vector()) + .def("set_position_upper_limits", &Robot::set_position_upper_limits, + py::arg("positions"), + py::arg("dof_names") = std::vector()) + .def("velocities", &Robot::velocities, py::arg("dof_names") = std::vector()) .def("set_velocities", &Robot::set_velocities, py::arg("velocities"), py::arg("dof_names") = std::vector()) + .def("velocity_lower_limits", &Robot::velocity_lower_limits, + py::arg("dof_names") = std::vector()) + .def("set_velocity_lower_limits", &Robot::set_velocity_lower_limits, + py::arg("velocities"), + py::arg("dof_names") = std::vector()) + .def("velocity_upper_limits", &Robot::velocity_upper_limits, + py::arg("dof_names") = std::vector()) + .def("set_velocity_upper_limits", &Robot::set_velocity_upper_limits, + py::arg("velocities"), + py::arg("dof_names") = std::vector()) + .def("accelerations", &Robot::accelerations, py::arg("dof_names") = std::vector()) .def("set_accelerations", &Robot::set_accelerations, py::arg("accelerations"), py::arg("dof_names") = std::vector()) + .def("acceleration_lower_limits", &Robot::acceleration_lower_limits, + py::arg("dof_names") = std::vector()) + .def("set_acceleration_lower_limits", &Robot::set_acceleration_lower_limits, + py::arg("accelerations"), + py::arg("dof_names") = std::vector()) + .def("acceleration_upper_limits", &Robot::acceleration_upper_limits, + py::arg("dof_names") = std::vector()) + .def("set_acceleration_upper_limits", &Robot::set_acceleration_upper_limits, + py::arg("accelerations"), + py::arg("dof_names") = std::vector()) + .def("forces", &Robot::forces, py::arg("dof_names") = std::vector()) .def("set_forces", &Robot::set_forces, py::arg("forces"), py::arg("dof_names") = std::vector()) + .def("force_lower_limits", &Robot::force_lower_limits, + py::arg("dof_names") = std::vector()) + .def("set_force_lower_limits", &Robot::set_force_lower_limits, + py::arg("forces"), + py::arg("dof_names") = std::vector()) + .def("force_upper_limits", &Robot::force_upper_limits, + py::arg("dof_names") = std::vector()) + .def("set_force_upper_limits", &Robot::set_force_upper_limits, + py::arg("forces"), + py::arg("dof_names") = std::vector()) + .def("commands", &Robot::commands, py::arg("dof_names") = std::vector()) .def("set_commands", &Robot::set_commands, diff --git a/src/robot_dart/robot.cpp b/src/robot_dart/robot.cpp index d5cd6c48..4771f5cd 100644 --- a/src/robot_dart/robot.cpp +++ b/src/robot_dart/robot.cpp @@ -47,6 +47,22 @@ namespace robot_dart { return skeleton->getForces(); else if (content == 4) return skeleton->getCommands(); + else if (content == 5) + return skeleton->getPositionLowerLimits(); + else if (content == 6) + return skeleton->getPositionUpperLimits(); + else if (content == 7) + return skeleton->getVelocityLowerLimits(); + else if (content == 8) + return skeleton->getVelocityUpperLimits(); + else if (content == 9) + return skeleton->getAccelerationLowerLimits(); + else if (content == 10) + return skeleton->getAccelerationUpperLimits(); + else if (content == 11) + return skeleton->getForceLowerLimits(); + else if (content == 12) + return skeleton->getForceUpperLimits(); ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!"); } @@ -66,6 +82,22 @@ namespace robot_dart { data(i) = dof->getForce(); else if (content == 4) data(i) = dof->getCommand(); + else if (content == 5) + data(i) = dof->getPositionLowerLimit(); + else if (content == 6) + data(i) = dof->getPositionUpperLimit(); + else if (content == 7) + data(i) = dof->getVelocityLowerLimit(); + else if (content == 8) + data(i) = dof->getVelocityUpperLimit(); + else if (content == 9) + data(i) = dof->getAccelerationLowerLimit(); + else if (content == 10) + data(i) = dof->getAccelerationUpperLimit(); + else if (content == 11) + data(i) = dof->getForceLowerLimit(); + else if (content == 12) + data(i) = dof->getForceUpperLimit(); else ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!"); } @@ -89,6 +121,22 @@ namespace robot_dart { return skeleton->setForces(data); else if (content == 4) return skeleton->setCommands(data); + else if (content == 5) + return skeleton->setPositionLowerLimits(data); + else if (content == 6) + return skeleton->setPositionUpperLimits(data); + else if (content == 7) + return skeleton->setVelocityLowerLimits(data); + else if (content == 8) + return skeleton->setVelocityUpperLimits(data); + else if (content == 9) + return skeleton->setAccelerationLowerLimits(data); + else if (content == 10) + return skeleton->setAccelerationUpperLimits(data); + else if (content == 11) + return skeleton->setForceLowerLimits(data); + else if (content == 12) + return skeleton->setForceUpperLimits(data); ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!"); } @@ -109,6 +157,22 @@ namespace robot_dart { dof->setForce(data(i)); else if (content == 4) dof->setCommand(data(i)); + else if (content == 5) + dof->setPositionLowerLimit(data(i)); + else if (content == 6) + dof->setPositionUpperLimit(data(i)); + else if (content == 7) + dof->setVelocityLowerLimit(data(i)); + else if (content == 8) + dof->setVelocityUpperLimit(data(i)); + else if (content == 9) + dof->setAccelerationLowerLimit(data(i)); + else if (content == 10) + dof->setAccelerationUpperLimit(data(i)); + else if (content == 11) + dof->setForceLowerLimit(data(i)); + else if (content == 12) + dof->setForceUpperLimit(data(i)); else ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!"); } @@ -131,6 +195,22 @@ namespace robot_dart { return skeleton->setForces(skeleton->getForces() + data); else if (content == 4) return skeleton->setCommands(skeleton->getCommands() + data); + else if (content == 5) + return skeleton->setPositionLowerLimits(skeleton->getPositionLowerLimits() + data); + else if (content == 6) + return skeleton->setPositionUpperLimits(skeleton->getPositionUpperLimits() + data); + else if (content == 7) + return skeleton->setVelocityLowerLimits(skeleton->getVelocityLowerLimits() + data); + else if (content == 8) + return skeleton->setVelocityUpperLimits(skeleton->getVelocityUpperLimits() + data); + else if (content == 9) + return skeleton->setAccelerationLowerLimits(skeleton->getAccelerationLowerLimits() + data); + else if (content == 10) + return skeleton->setAccelerationUpperLimits(skeleton->getAccelerationUpperLimits() + data); + else if (content == 11) + return skeleton->setForceLowerLimits(skeleton->getForceLowerLimits() + data); + else if (content == 12) + return skeleton->setForceUpperLimits(skeleton->getForceUpperLimits() + data); ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!"); } @@ -151,6 +231,22 @@ namespace robot_dart { dof->setForce(dof->getForce() + data(i)); else if (content == 4) dof->setCommand(dof->getCommand() + data(i)); + else if (content == 5) + dof->setPositionLowerLimit(dof->getPositionLowerLimit() + data(i)); + else if (content == 6) + dof->setPositionUpperLimit(dof->getPositionUpperLimit() + data(i)); + else if (content == 7) + dof->setVelocityLowerLimit(dof->getVelocityLowerLimit() + data(i)); + else if (content == 8) + dof->setVelocityUpperLimit(dof->getVelocityUpperLimit() + data(i)); + else if (content == 9) + dof->setAccelerationLowerLimit(dof->getAccelerationLowerLimit() + data(i)); + else if (content == 10) + dof->setAccelerationUpperLimit(dof->getAccelerationUpperLimit() + data(i)); + else if (content == 11) + dof->setForceLowerLimit(dof->getForceLowerLimit() + data(i)); + else if (content == 12) + dof->setForceUpperLimit(dof->getForceUpperLimit() + data(i)); else ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!"); } @@ -370,6 +466,14 @@ namespace robot_dart { return parent_jt->getType() == dart::dynamics::FreeJoint::getStaticType(); } + void Robot::reset() + { + _skeleton->resetPositions(); + _skeleton->resetVelocities(); + _skeleton->resetAccelerations(); + this->clear_external_forces(); + } + void Robot::set_actuator_types(const std::string& type, const std::vector& joint_names, bool override_mimic, bool override_base) { // Set all dofs to same actuator type @@ -662,7 +766,7 @@ namespace robot_dart { return _skeleton->getCOMSpatialAcceleration(); } - Eigen::VectorXd Robot::positions(const std::vector& dof_names) + Eigen::VectorXd Robot::positions(const std::vector& dof_names) const { return detail::dof_data<0>(_skeleton, dof_names, _dof_map); } @@ -672,7 +776,27 @@ namespace robot_dart { detail::set_dof_data<0>(positions, _skeleton, dof_names, _dof_map); } - Eigen::VectorXd Robot::velocities(const std::vector& dof_names) + Eigen::VectorXd Robot::position_lower_limits(const std::vector& dof_names) const + { + return detail::dof_data<5>(_skeleton, dof_names, _dof_map); + } + + void Robot::set_position_lower_limits(const Eigen::VectorXd& positions, const std::vector& dof_names) + { + detail::set_dof_data<5>(positions, _skeleton, dof_names, _dof_map); + } + + Eigen::VectorXd Robot::position_upper_limits(const std::vector& dof_names) const + { + return detail::dof_data<6>(_skeleton, dof_names, _dof_map); + } + + void Robot::set_position_upper_limits(const Eigen::VectorXd& positions, const std::vector& dof_names) + { + detail::set_dof_data<6>(positions, _skeleton, dof_names, _dof_map); + } + + Eigen::VectorXd Robot::velocities(const std::vector& dof_names) const { return detail::dof_data<1>(_skeleton, dof_names, _dof_map); } @@ -682,7 +806,27 @@ namespace robot_dart { detail::set_dof_data<1>(velocities, _skeleton, dof_names, _dof_map); } - Eigen::VectorXd Robot::accelerations(const std::vector& dof_names) + Eigen::VectorXd Robot::velocity_lower_limits(const std::vector& dof_names) const + { + return detail::dof_data<7>(_skeleton, dof_names, _dof_map); + } + + void Robot::set_velocity_lower_limits(const Eigen::VectorXd& velocities, const std::vector& dof_names) + { + detail::set_dof_data<7>(velocities, _skeleton, dof_names, _dof_map); + } + + Eigen::VectorXd Robot::velocity_upper_limits(const std::vector& dof_names) const + { + return detail::dof_data<8>(_skeleton, dof_names, _dof_map); + } + + void Robot::set_velocity_upper_limits(const Eigen::VectorXd& velocities, const std::vector& dof_names) + { + detail::set_dof_data<8>(velocities, _skeleton, dof_names, _dof_map); + } + + Eigen::VectorXd Robot::accelerations(const std::vector& dof_names) const { return detail::dof_data<2>(_skeleton, dof_names, _dof_map); } @@ -692,7 +836,27 @@ namespace robot_dart { detail::set_dof_data<2>(accelerations, _skeleton, dof_names, _dof_map); } - Eigen::VectorXd Robot::forces(const std::vector& dof_names) + Eigen::VectorXd Robot::acceleration_lower_limits(const std::vector& dof_names) const + { + return detail::dof_data<9>(_skeleton, dof_names, _dof_map); + } + + void Robot::set_acceleration_lower_limits(const Eigen::VectorXd& accelerations, const std::vector& dof_names) + { + detail::set_dof_data<9>(accelerations, _skeleton, dof_names, _dof_map); + } + + Eigen::VectorXd Robot::acceleration_upper_limits(const std::vector& dof_names) const + { + return detail::dof_data<10>(_skeleton, dof_names, _dof_map); + } + + void Robot::set_acceleration_upper_limits(const Eigen::VectorXd& accelerations, const std::vector& dof_names) + { + detail::set_dof_data<10>(accelerations, _skeleton, dof_names, _dof_map); + } + + Eigen::VectorXd Robot::forces(const std::vector& dof_names) const { return detail::dof_data<3>(_skeleton, dof_names, _dof_map); } @@ -702,7 +866,27 @@ namespace robot_dart { detail::set_dof_data<3>(forces, _skeleton, dof_names, _dof_map); } - Eigen::VectorXd Robot::commands(const std::vector& dof_names) + Eigen::VectorXd Robot::force_lower_limits(const std::vector& dof_names) const + { + return detail::dof_data<11>(_skeleton, dof_names, _dof_map); + } + + void Robot::set_force_lower_limits(const Eigen::VectorXd& forces, const std::vector& dof_names) + { + detail::set_dof_data<11>(forces, _skeleton, dof_names, _dof_map); + } + + Eigen::VectorXd Robot::force_upper_limits(const std::vector& dof_names) const + { + return detail::dof_data<12>(_skeleton, dof_names, _dof_map); + } + + void Robot::set_force_upper_limits(const Eigen::VectorXd& forces, const std::vector& dof_names) + { + detail::set_dof_data<12>(forces, _skeleton, dof_names, _dof_map); + } + + Eigen::VectorXd Robot::commands(const std::vector& dof_names) const { return detail::dof_data<4>(_skeleton, dof_names, _dof_map); } diff --git a/src/robot_dart/robot.hpp b/src/robot_dart/robot.hpp index 179a1031..e1f59aff 100644 --- a/src/robot_dart/robot.hpp +++ b/src/robot_dart/robot.hpp @@ -63,6 +63,8 @@ namespace robot_dart { bool fixed() const; bool free() const; + void reset(); + // actuator type can be: torque, servo, velocity, passive, locked, mimic (only for completeness, use set_mimic to use this) // Be careful that actuator types are per joint and not per DoF void set_actuator_types(const std::string& type, const std::vector& joint_names = {}, bool override_mimic = false, bool override_base = false); @@ -106,19 +108,39 @@ namespace robot_dart { Eigen::Vector6d com_velocity() const; Eigen::Vector6d com_acceleration() const; - Eigen::VectorXd positions(const std::vector& dof_names = {}); + Eigen::VectorXd positions(const std::vector& dof_names = {}) const; void set_positions(const Eigen::VectorXd& positions, const std::vector& dof_names = {}); - Eigen::VectorXd velocities(const std::vector& dof_names = {}); + Eigen::VectorXd position_lower_limits(const std::vector& dof_names = {}) const; + void set_position_lower_limits(const Eigen::VectorXd& positions, const std::vector& dof_names = {}); + Eigen::VectorXd position_upper_limits(const std::vector& dof_names = {}) const; + void set_position_upper_limits(const Eigen::VectorXd& positions, const std::vector& dof_names = {}); + + Eigen::VectorXd velocities(const std::vector& dof_names = {}) const; void set_velocities(const Eigen::VectorXd& velocities, const std::vector& dof_names = {}); - Eigen::VectorXd accelerations(const std::vector& dof_names = {}); + Eigen::VectorXd velocity_lower_limits(const std::vector& dof_names = {}) const; + void set_velocity_lower_limits(const Eigen::VectorXd& velocities, const std::vector& dof_names = {}); + Eigen::VectorXd velocity_upper_limits(const std::vector& dof_names = {}) const; + void set_velocity_upper_limits(const Eigen::VectorXd& velocities, const std::vector& dof_names = {}); + + Eigen::VectorXd accelerations(const std::vector& dof_names = {}) const; void set_accelerations(const Eigen::VectorXd& accelerations, const std::vector& dof_names = {}); - Eigen::VectorXd forces(const std::vector& dof_names = {}); + Eigen::VectorXd acceleration_lower_limits(const std::vector& dof_names = {}) const; + void set_acceleration_lower_limits(const Eigen::VectorXd& accelerations, const std::vector& dof_names = {}); + Eigen::VectorXd acceleration_upper_limits(const std::vector& dof_names = {}) const; + void set_acceleration_upper_limits(const Eigen::VectorXd& accelerations, const std::vector& dof_names = {}); + + Eigen::VectorXd forces(const std::vector& dof_names = {}) const; void set_forces(const Eigen::VectorXd& forces, const std::vector& dof_names = {}); - Eigen::VectorXd commands(const std::vector& dof_names = {}); + Eigen::VectorXd force_lower_limits(const std::vector& dof_names = {}) const; + void set_force_lower_limits(const Eigen::VectorXd& forces, const std::vector& dof_names = {}); + Eigen::VectorXd force_upper_limits(const std::vector& dof_names = {}) const; + void set_force_upper_limits(const Eigen::VectorXd& forces, const std::vector& dof_names = {}); + + Eigen::VectorXd commands(const std::vector& dof_names = {}) const; void set_commands(const Eigen::VectorXd& commands, const std::vector& dof_names = {}); std::pair force_torque(size_t joint_index) const;