Merge pull request #76 from ARC-OPT/scene_plugins

Scene plugins

bindings/python/examples/kuka_iiwa/cart_pos_ctrl_hls_solver.py

@@ -1,6 +1,6 @@
 from wbc.core import *
 from wbc.robot_models.robot_model_rbdl import RobotModelRBDL
-from wbc.scenes import VelocityScene
+from wbc.scenes.velocity_scene import VelocityScene
 from wbc.solvers.hls_solver import HierarchicalLSSolver
 from wbc.controllers import CartesianPosPDController
 import time
@@ -32,7 +32,7 @@
 cfg.weights = [1]*6
 
 # Configure WBC Scene
-scene=VelocityScene(robot_model, solver)
+scene=VelocityScene(robot_model, solver, 0.001)
 if scene.configure([cfg]) == False:
     print("Failed to configure scene")
     exit(0)

bindings/python/examples/rh5/rh5_legs_floating_base.py

@@ -1,6 +1,6 @@
 from wbc.core import *
 from wbc.robot_models.robot_model_rbdl import RobotModelRBDL
-from wbc.scenes import AccelerationSceneTSID
+from wbc.scenes.acceleration_scene_tsid import AccelerationSceneTSID
 from wbc.solvers.qpoases_solver import QPOASESSolver
 from wbc.controllers import CartesianPosPDController
 import time
@@ -44,7 +44,7 @@
 cfg.weights = [1]*6
 
 # Configure WBC Scene
-scene=AccelerationSceneTSID(robot_model, solver)
+scene=AccelerationSceneTSID(robot_model, solver, 0.001)
 if scene.configure([cfg]) == False:
     print("Failed to configure scene")
     exit(0)
@@ -90,12 +90,12 @@
 scene.setReference(cfg.name,control_output)
 qp = scene.update()
 solver_output   = scene.solve(qp)
-wrenches_output = scene.getContactWrenches()
+#wrenches_output = scene.getContactWrenches()
 
 print("----- Solver output -----")
 print("Names: " + str(robot_model.actuatedJointNames()))
 print("Acc:   " + str([e.acceleration for e in solver_output.elements]))
 print("Tau:   " + str([e.effort for e in solver_output.elements]))
-print("Contact Wrenches")
-for name,elem in zip(wrenches_output.names, wrenches_output.elements):
-    print(name + ": Force " + str(elem.force.transpose()) + ", Torque " + str(elem.torque.transpose()))
+#print("Contact Wrenches")
+#for name,elem in zip(wrenches_output.names, wrenches_output.elements):
+#    print(name + ": Force " + str(elem.force.transpose()) + ", Torque " + str(elem.torque.transpose()))

bindings/python/scenes/CMakeLists.txt

@@ -1,16 +1,8 @@
-PYTHON_ADD_MODULE(scenes scenes.cpp)
+add_subdirectory(velocity)
+add_subdirectory(velocity_qp)
+add_subdirectory(acceleration)
+add_subdirectory(acceleration_tsid)
+add_subdirectory(acceleration_reduced_tsid)
 
-# Set up the libraries and header search paths for this target
-target_link_libraries(scenes PUBLIC
-	wbc-scenes
-        wbc-robot_models-rbdl
-	wbc-solvers-hls
-        wbc-solvers-qpoases
-        Boost::python
-        Boost::numpy)
-
-target_include_directories(scenes PUBLIC
-	${PYTHON_INCLUDE_DIRS})
-
-install(TARGETS scenes
-        DESTINATION ${PYTHON_INSTALL_PATH}/wbc)
+install(FILES __init__.py
+        DESTINATION ${PYTHON_INSTALL_PATH}/wbc/scenes)

bindings/python/scenes/acceleration/CMakeLists.txt

@@ -0,0 +1,15 @@
+PYTHON_ADD_MODULE(acceleration_scene acceleration_scene.cpp)
+
+# Set up the libraries and header search paths for this target
+target_link_libraries(acceleration_scene PUBLIC
+	      wbc-scenes-acceleration
+        wbc-robot_models-rbdl
+	      wbc-solvers-qpoases
+        Boost::python
+        Boost::numpy)
+
+target_include_directories(acceleration_scene PUBLIC
+	${PYTHON_INCLUDE_DIRS})
+
+install(TARGETS acceleration_scene
+        DESTINATION ${PYTHON_INSTALL_PATH}/wbc/scenes)

bindings/python/scenes/acceleration/acceleration_scene.cpp

@@ -0,0 +1,84 @@
+#include "../../eigen_conversion.h"
+#include "../../base_types_conversion.h"
+#include "../../std_vector_conversion.h"
+#include "acceleration_scene.hpp"
+
+namespace wbc_py {
+
+wbc::JointWeights toJointWeights(const base::NamedVector<double> &weights){
+    wbc::JointWeights weights_out;
+    weights_out.elements = weights.elements;
+    weights_out.names = weights.names;
+    return weights_out;
+}
+
+base::NamedVector<double> toNamedVector(const wbc::JointWeights &weights){
+    base::NamedVector<double> weights_out;
+    weights_out.elements = weights.elements;
+    weights_out.names = weights.names;
+    return weights_out;
+}
+
+base::NamedVector<wbc::TaskStatus> toNamedVector(const wbc::TasksStatus& status_in){
+    base::NamedVector<wbc::TaskStatus> status_out;
+    status_out.elements = status_in.elements;
+    status_out.names = status_in.names;
+    return status_out;
+}
+
+base::NamedVector<base::Wrench> toNamedVector(const base::samples::Wrenches& wrenches_in){
+    base::NamedVector<base::Wrench> wrenches_out;
+    wrenches_out.elements = wrenches_in.elements;
+    wrenches_out.names = wrenches_in.names;
+    return wrenches_out;
+}
+
+AccelerationScene::AccelerationScene(std::shared_ptr<RobotModelRBDL> robot_model, std::shared_ptr<QPOASESSolver> solver, const double dt) :
+    wbc::AccelerationScene(robot_model, solver, dt){
+}
+void AccelerationScene::setJointReference(const std::string& task_name, const base::NamedVector<base::JointState>& ref){
+    wbc::AccelerationScene::setReference(task_name, tobaseSamplesJoints(ref));
+}
+void AccelerationScene::setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref){
+    wbc::AccelerationScene::setReference(task_name, ref);
+}
+void AccelerationScene::setJointWeights(const base::NamedVector<double> &weights){
+    wbc::AccelerationScene::setJointWeights(toJointWeights(weights));
+}
+base::NamedVector<double> AccelerationScene::getJointWeights2(){
+    return toNamedVector(wbc::AccelerationScene::getJointWeights());
+}
+base::NamedVector<double> AccelerationScene::getActuatedJointWeights2(){
+    return toNamedVector(wbc::AccelerationScene::getActuatedJointWeights());
+}
+base::NamedVector<base::JointState> AccelerationScene::solve2(const wbc::HierarchicalQP &hqp){
+    return toNamedVector(wbc::AccelerationScene::solve(hqp));
+}
+base::NamedVector<wbc::TaskStatus> AccelerationScene::updateTasksStatus2(){
+    return toNamedVector(wbc::AccelerationScene::updateTasksStatus());
+}
+
+}
+
+BOOST_PYTHON_MODULE(acceleration_scene){
+
+    np::initialize();
+
+    py::class_<wbc_py::AccelerationScene>("AccelerationScene", py::init<std::shared_ptr<wbc_py::RobotModelRBDL>,std::shared_ptr<wbc_py::QPOASESSolver>,const double>())
+            .def("configure",    &wbc_py::AccelerationScene::configure)
+            .def("update",       &wbc_py::AccelerationScene::update, py::return_value_policy<py::copy_const_reference>())
+            .def("solve",        &wbc_py::AccelerationScene::solve2)
+            .def("setReference", &wbc_py::AccelerationScene::setJointReference)
+            .def("setReference", &wbc_py::AccelerationScene::setCartReference)
+            .def("setTaskWeights",   &wbc_py::AccelerationScene::setTaskWeights)
+            .def("setTaskActivation",   &wbc_py::AccelerationScene::setTaskActivation)
+            .def("getTasksStatus",   &wbc_py::AccelerationScene::getTasksStatus,  py::return_value_policy<py::copy_const_reference>())
+            .def("getNConstraintVariablesPerPrio",   &wbc_py::AccelerationScene::getNTaskVariablesPerPrio)
+            .def("hasTask",   &wbc_py::AccelerationScene::hasTask)
+            .def("updateTasksStatus",   &wbc_py::AccelerationScene::updateTasksStatus2)
+            .def("getHierarchicalQP",   &wbc_py::AccelerationScene::getHierarchicalQP,  py::return_value_policy<py::copy_const_reference>())
+            .def("getSolverOutput",   &wbc_py::AccelerationScene::getSolverOutput,  py::return_value_policy<py::copy_const_reference>())
+            .def("setJointWeights",   &wbc_py::AccelerationScene::setJointWeights)
+            .def("getJointWeights",   &wbc_py::AccelerationScene::getJointWeights2)
+            .def("getActuatedJointWeights",   &wbc_py::AccelerationScene::getActuatedJointWeights2);
+}

bindings/python/scenes/acceleration/acceleration_scene.hpp

@@ -0,0 +1,26 @@
+#ifndef WBC_PY_ACCELERATION_SCENE_HPP
+#define WBC_PY_ACCELERATION_SCENE_HPP
+
+#include "scenes/acceleration/AccelerationScene.hpp"
+#include "../../solvers/qpoases/QPOasesSolver.hpp"
+#include "../../robot_models/rbdl/robot_model_rbdl.hpp"
+
+namespace wbc_py {
+
+wbc::JointWeights toJointWeights(const base::NamedVector<double> &weights);
+
+class AccelerationScene : public wbc::AccelerationScene{
+public:
+    AccelerationScene(std::shared_ptr<RobotModelRBDL> robot_model, std::shared_ptr<QPOASESSolver> solver, const double dt);
+    void setJointReference(const std::string& task_name, const base::NamedVector<base::JointState>& ref);
+    void setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref);
+    void setJointWeights(const base::NamedVector<double> &weights);
+    base::NamedVector<double> getJointWeights2();
+    base::NamedVector<double> getActuatedJointWeights2();
+    base::NamedVector<base::JointState> solve2(const wbc::HierarchicalQP &hqp);
+    base::NamedVector<wbc::TaskStatus> updateTasksStatus2();
+};
+
+}
+
+#endif

bindings/python/scenes/acceleration_reduced_tsid/CMakeLists.txt

@@ -0,0 +1,15 @@
+PYTHON_ADD_MODULE(acceleration_scene_reduced_tsid acceleration_scene_reduced_tsid.cpp)
+
+# Set up the libraries and header search paths for this target
+target_link_libraries(acceleration_scene_reduced_tsid PUBLIC
+	      wbc-scenes-acceleration_reduced_tsid
+        wbc-robot_models-rbdl
+	      wbc-solvers-qpoases
+        Boost::python
+        Boost::numpy)
+
+target_include_directories(acceleration_scene_reduced_tsid PUBLIC
+	${PYTHON_INCLUDE_DIRS})
+
+install(TARGETS acceleration_scene_reduced_tsid
+        DESTINATION ${PYTHON_INSTALL_PATH}/wbc/scenes)

bindings/python/scenes/acceleration_reduced_tsid/acceleration_scene_reduced_tsid.cpp

@@ -0,0 +1,84 @@
+#include "../../eigen_conversion.h"
+#include "../../base_types_conversion.h"
+#include "../../std_vector_conversion.h"
+#include "acceleration_scene_reduced_tsid.hpp"
+
+namespace wbc_py {
+
+wbc::JointWeights toJointWeights(const base::NamedVector<double> &weights){
+    wbc::JointWeights weights_out;
+    weights_out.elements = weights.elements;
+    weights_out.names = weights.names;
+    return weights_out;
+}
+
+base::NamedVector<double> toNamedVector(const wbc::JointWeights &weights){
+    base::NamedVector<double> weights_out;
+    weights_out.elements = weights.elements;
+    weights_out.names = weights.names;
+    return weights_out;
+}
+
+base::NamedVector<wbc::TaskStatus> toNamedVector(const wbc::TasksStatus& status_in){
+    base::NamedVector<wbc::TaskStatus> status_out;
+    status_out.elements = status_in.elements;
+    status_out.names = status_in.names;
+    return status_out;
+}
+
+base::NamedVector<base::Wrench> toNamedVector(const base::samples::Wrenches& wrenches_in){
+    base::NamedVector<base::Wrench> wrenches_out;
+    wrenches_out.elements = wrenches_in.elements;
+    wrenches_out.names = wrenches_in.names;
+    return wrenches_out;
+}
+
+AccelerationSceneReducedTSID::AccelerationSceneReducedTSID(std::shared_ptr<RobotModelRBDL> robot_model, std::shared_ptr<QPOASESSolver> solver, const double dt) :
+    wbc::AccelerationSceneReducedTSID(robot_model, solver, dt){
+}
+void AccelerationSceneReducedTSID::setJointReference(const std::string& task_name, const base::NamedVector<base::JointState>& ref){
+    wbc::AccelerationSceneReducedTSID::setReference(task_name, tobaseSamplesJoints(ref));
+}
+void AccelerationSceneReducedTSID::setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref){
+    wbc::AccelerationSceneReducedTSID::setReference(task_name, ref);
+}
+void AccelerationSceneReducedTSID::setJointWeights(const base::NamedVector<double> &weights){
+    wbc::AccelerationSceneReducedTSID::setJointWeights(toJointWeights(weights));
+}
+base::NamedVector<double> AccelerationSceneReducedTSID::getJointWeights2(){
+    return toNamedVector(wbc::AccelerationSceneReducedTSID::getJointWeights());
+}
+base::NamedVector<double> AccelerationSceneReducedTSID::getActuatedJointWeights2(){
+    return toNamedVector(wbc::AccelerationSceneReducedTSID::getActuatedJointWeights());
+}
+base::NamedVector<base::JointState> AccelerationSceneReducedTSID::solve2(const wbc::HierarchicalQP &hqp){
+    return toNamedVector(wbc::AccelerationSceneReducedTSID::solve(hqp));
+}
+base::NamedVector<wbc::TaskStatus> AccelerationSceneReducedTSID::updateTasksStatus2(){
+    return toNamedVector(wbc::AccelerationSceneReducedTSID::updateTasksStatus());
+}
+
+}
+
+BOOST_PYTHON_MODULE(acceleration_scene_reduced_tsid){
+
+    np::initialize();
+
+    py::class_<wbc_py::AccelerationSceneReducedTSID>("AccelerationSceneReducedTSID", py::init<std::shared_ptr<wbc_py::RobotModelRBDL>,std::shared_ptr<wbc_py::QPOASESSolver>,const double>())
+            .def("configure",    &wbc_py::AccelerationSceneReducedTSID::configure)
+            .def("update",       &wbc_py::AccelerationSceneReducedTSID::update, py::return_value_policy<py::copy_const_reference>())
+            .def("solve",        &wbc_py::AccelerationSceneReducedTSID::solve2)
+            .def("setReference", &wbc_py::AccelerationSceneReducedTSID::setJointReference)
+            .def("setReference", &wbc_py::AccelerationSceneReducedTSID::setCartReference)
+            .def("setTaskWeights",   &wbc_py::AccelerationSceneReducedTSID::setTaskWeights)
+            .def("setTaskActivation",   &wbc_py::AccelerationSceneReducedTSID::setTaskActivation)
+            .def("getTasksStatus",   &wbc_py::AccelerationSceneReducedTSID::getTasksStatus,  py::return_value_policy<py::copy_const_reference>())
+            .def("getNConstraintVariablesPerPrio",   &wbc_py::AccelerationSceneReducedTSID::getNTaskVariablesPerPrio)
+            .def("hasTask",   &wbc_py::AccelerationSceneReducedTSID::hasTask)
+            .def("updateTasksStatus",   &wbc_py::AccelerationSceneReducedTSID::updateTasksStatus2)
+            .def("getHierarchicalQP",   &wbc_py::AccelerationSceneReducedTSID::getHierarchicalQP,  py::return_value_policy<py::copy_const_reference>())
+            .def("getSolverOutput",   &wbc_py::AccelerationSceneReducedTSID::getSolverOutput,  py::return_value_policy<py::copy_const_reference>())
+            .def("setJointWeights",   &wbc_py::AccelerationSceneReducedTSID::setJointWeights)
+            .def("getJointWeights",   &wbc_py::AccelerationSceneReducedTSID::getJointWeights2)
+            .def("getActuatedJointWeights",   &wbc_py::AccelerationSceneReducedTSID::getActuatedJointWeights2);
+}

bindings/python/scenes/acceleration_reduced_tsid/acceleration_scene_reduced_tsid.hpp

@@ -0,0 +1,26 @@
+#ifndef WBC_PY_ACCELERATION_SCENE_REDUCED_TSID_HPP
+#define WBC_PY_ACCELERATION_SCENE_REDUCED_TSID_HPP
+
+#include "scenes/acceleration_reduced_tsid/AccelerationSceneReducedTSID.hpp"
+#include "../../solvers/qpoases/QPOasesSolver.hpp"
+#include "../../robot_models/rbdl/robot_model_rbdl.hpp"
+
+namespace wbc_py {
+
+wbc::JointWeights toJointWeights(const base::NamedVector<double> &weights);
+
+class AccelerationSceneReducedTSID : public wbc::AccelerationSceneReducedTSID{
+public:
+    AccelerationSceneReducedTSID(std::shared_ptr<RobotModelRBDL> robot_model, std::shared_ptr<QPOASESSolver> solver, const double dt);
+    void setJointReference(const std::string& task_name, const base::NamedVector<base::JointState>& ref);
+    void setCartReference(const std::string& task_name, const base::samples::RigidBodyStateSE3& ref);
+    void setJointWeights(const base::NamedVector<double> &weights);
+    base::NamedVector<double> getJointWeights2();
+    base::NamedVector<double> getActuatedJointWeights2();
+    base::NamedVector<base::JointState> solve2(const wbc::HierarchicalQP &hqp);
+    base::NamedVector<wbc::TaskStatus> updateTasksStatus2();
+};
+
+}
+
+#endif

bindings/python/scenes/acceleration_tsid/CMakeLists.txt

@@ -0,0 +1,15 @@
+PYTHON_ADD_MODULE(acceleration_scene_tsid acceleration_scene_tsid.cpp)
+
+# Set up the libraries and header search paths for this target
+target_link_libraries(acceleration_scene_tsid PUBLIC
+	      wbc-scenes-acceleration_tsid
+        wbc-robot_models-rbdl
+	      wbc-solvers-qpoases
+        Boost::python
+        Boost::numpy)
+
+target_include_directories(acceleration_scene_tsid PUBLIC
+	${PYTHON_INCLUDE_DIRS})
+
+install(TARGETS acceleration_scene_tsid
+        DESTINATION ${PYTHON_INSTALL_PATH}/wbc/scenes)