started reflexxes based trajectory generation module

.gitignore

@@ -0,0 +1,8 @@
+templates/*
+.orogen/*
+*~
+.*.swp
+build/*
+
+*autobuild-stamp
+

CMakeLists.txt

@@ -0,0 +1,9 @@
+PROJECT(trajectory_generation)
+cmake_minimum_required(VERSION 2.6)
+
+SET (CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/.orogen/config")
+INCLUDE(trajectory_generationBase)
+
+# FIND_PACKAGE(KDL)
+# FIND_PACKAGE(OCL)
+

manifest.xml

@@ -0,0 +1,9 @@
+<package>
+  <description brief="Instantaneous trajectory generation component">
+  This component uses the reflexxes library to generate motion commands based on the current state of the system, the target state and motion constraints.
+  </description>
+  <author>Jakob Schwendner/[email protected]</author>
+  <license>LGPL</license>
+  <depend package="base/types" />
+  <depend package="reflexxes" />
+</package>

tasks/CMakeLists.txt

@@ -0,0 +1,21 @@
+# Generated from orogen/lib/orogen/templates/tasks/CMakeLists.txt
+
+include(trajectory_generationTaskLib)
+ADD_LIBRARY(${TRAJECTORY_GENERATION_TASKLIB_NAME} SHARED 
+    ${TRAJECTORY_GENERATION_TASKLIB_SOURCES})
+add_dependencies(${TRAJECTORY_GENERATION_TASKLIB_NAME}
+    regen-typekit)
+
+TARGET_LINK_LIBRARIES(${TRAJECTORY_GENERATION_TASKLIB_NAME}
+    ${OrocosRTT_LIBRARIES}
+    ${TRAJECTORY_GENERATION_TASKLIB_DEPENDENT_LIBRARIES})
+SET_TARGET_PROPERTIES(${TRAJECTORY_GENERATION_TASKLIB_NAME}
+    PROPERTIES LINK_INTERFACE_LIBRARIES "${TRAJECTORY_GENERATION_TASKLIB_INTERFACE_LIBRARIES}")
+
+INSTALL(TARGETS ${TRAJECTORY_GENERATION_TASKLIB_NAME}
+    RUNTIME DESTINATION bin
+    LIBRARY DESTINATION lib/orocos)
+
+INSTALL(FILES ${TRAJECTORY_GENERATION_TASKLIB_HEADERS}
+    DESTINATION include/orocos/trajectory_generation)
+

tasks/Task.cpp

@@ -0,0 +1,66 @@
+/* Generated from orogen/lib/orogen/templates/tasks/Task.cpp */
+
+#include "Task.hpp"
+
+using namespace trajectory_generation;
+
+Task::Task(std::string const& name)
+    : TaskBase(name)
+{
+}
+
+Task::Task(std::string const& name, RTT::ExecutionEngine* engine)
+    : TaskBase(name, engine)
+{
+}
+
+Task::~Task()
+{
+}
+
+
+
+/// The following lines are template definitions for the various state machine
+// hooks defined by Orocos::RTT. See Task.hpp for more detailed
+// documentation about them.
+
+bool Task::configureHook()
+{
+    if (! TaskBase::configureHook())
+        return false;
+
+    const int NUMBER_OF_DOFS = 6;
+    const double CYCLE_TIME_IN_SECONDS = 0.01;
+
+    RML = new ReflexxesAPI( NUMBER_OF_DOFS, CYCLE_TIME_IN_SECONDS );
+    IP  = new RMLPositionInputParameters( NUMBER_OF_DOFS );
+    OP  = new RMLPositionOutputParameters( NUMBER_OF_DOFS );
+
+    return true;
+}
+bool Task::startHook()
+{
+    if (! TaskBase::startHook())
+        return false;
+    return true;
+}
+void Task::updateHook()
+{
+    TaskBase::updateHook();
+}
+void Task::errorHook()
+{
+    TaskBase::errorHook();
+}
+void Task::stopHook()
+{
+    TaskBase::stopHook();
+}
+void Task::cleanupHook()
+{
+    delete RML;
+    delete IP;
+    delete OP;
+
+    TaskBase::cleanupHook();
+}

tasks/Task.hpp

@@ -0,0 +1,118 @@
+/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */
+
+#ifndef TRAJECTORY_GENERATION_TASK_TASK_HPP
+#define TRAJECTORY_GENERATION_TASK_TASK_HPP
+
+#include "trajectory_generation/TaskBase.hpp"
+
+#include <ReflexxesAPI.h>
+#include <RMLPositionFlags.h>
+#include <RMLPositionInputParameters.h>
+#include <RMLPositionOutputParameters.h>
+
+namespace trajectory_generation {
+
+    /*! \class Task 
+     * \brief The task context provides and requires services. It uses an ExecutionEngine to perform its functions.
+     * Essential interfaces are operations, data flow ports and properties. These interfaces have been defined using the oroGen specification.
+     * In order to modify the interfaces you should (re)use oroGen and rely on the associated workflow.
+     * 
+     * \details
+     * The name of a TaskContext is primarily defined via:
+     \verbatim
+     deployment 'deployment_name'
+         task('custom_task_name','trajectory_generation::Task')
+     end
+     \endverbatim
+     *  It can be dynamically adapted when the deployment is called with a prefix argument. 
+     */
+    class Task : public TaskBase
+    {
+	friend class TaskBase;
+    protected:
+
+	ReflexxesAPI *RML;
+	RMLPositionInputParameters *IP;
+	RMLPositionOutputParameters *OP;
+	RMLPositionFlags Flags;
+
+    public:
+        /** TaskContext constructor for Task
+         * \param name Name of the task. This name needs to be unique to make it identifiable via nameservices.
+         * \param initial_state The initial TaskState of the TaskContext. Default is Stopped state.
+         */
+        Task(std::string const& name = "trajectory_generation::Task");
+
+        /** TaskContext constructor for Task 
+         * \param name Name of the task. This name needs to be unique to make it identifiable for nameservices. 
+         * \param engine The RTT Execution engine to be used for this task, which serialises the execution of all commands, programs, state machines and incoming events for a task. 
+         * 
+         */
+        Task(std::string const& name, RTT::ExecutionEngine* engine);
+
+        /** Default deconstructor of Task
+         */
+	~Task();
+
+        /** This hook is called by Orocos when the state machine transitions
+         * from PreOperational to Stopped. If it returns false, then the
+         * component will stay in PreOperational. Otherwise, it goes into
+         * Stopped.
+         *
+         * It is meaningful only if the #needs_configuration has been specified
+         * in the task context definition with (for example):
+         \verbatim
+         task_context "TaskName" do
+           needs_configuration
+           ...
+         end
+         \endverbatim
+         */
+        bool configureHook();
+
+        /** This hook is called by Orocos when the state machine transitions
+         * from Stopped to Running. If it returns false, then the component will
+         * stay in Stopped. Otherwise, it goes into Running and updateHook()
+         * will be called.
+         */
+        bool startHook();
+
+        /** This hook is called by Orocos when the component is in the Running
+         * state, at each activity step. Here, the activity gives the "ticks"
+         * when the hook should be called.
+         *
+         * The error(), exception() and fatal() calls, when called in this hook,
+         * allow to get into the associated RunTimeError, Exception and
+         * FatalError states. 
+         *
+         * In the first case, updateHook() is still called, and recover() allows
+         * you to go back into the Running state.  In the second case, the
+         * errorHook() will be called instead of updateHook(). In Exception, the
+         * component is stopped and recover() needs to be called before starting
+         * it again. Finally, FatalError cannot be recovered.
+         */
+        void updateHook();
+
+        /** This hook is called by Orocos when the component is in the
+         * RunTimeError state, at each activity step. See the discussion in
+         * updateHook() about triggering options.
+         *
+         * Call recover() to go back in the Runtime state.
+         */
+        void errorHook();
+
+        /** This hook is called by Orocos when the state machine transitions
+         * from Running to Stopped after stop() has been called.
+         */
+        void stopHook();
+
+        /** This hook is called by Orocos when the state machine transitions
+         * from Stopped to PreOperational, requiring the call to configureHook()
+         * before calling start() again.
+         */
+        void cleanupHook();
+    };
+}
+
+#endif
+

trajectory_generation.orogen

@@ -0,0 +1,24 @@
+name "trajectory_generation"
+
+import_types_from "trajectory_generationTypes.hpp"
+import_types_from "base"
+using_library "reflexxes"
+
+task_context "Task" do
+    needs_configuration
+
+    input_port("limits", "/base/JointLimits").
+	doc("The limits for the joints")
+
+    input_port("target", "/base/commands/JointsSampledTrajectory").
+	doc("The target trajectory which should be followed")
+
+    input_port("joint_state", "/base/samples/Joints").
+	doc("Current state measurement from the actuators")
+
+    output_port("cmd", "/base/commands/Joints").
+	doc("Target values that are send to the actuators")
+
+    periodic 0.1
+end
+

trajectory_generationTypes.hpp

@@ -0,0 +1,15 @@
+#ifndef trajectory_generation_TYPES_HPP
+#define trajectory_generation_TYPES_HPP
+
+/* If you need to define types specific to your oroGen components, define them
+ * here. Required headers must be included explicitly
+ *
+ * However, it is common that you will only import types from your library, in
+ * which case you do not need this file
+ */
+
+namespace trajectory_generation {
+}
+
+#endif
+