Commit b4706249 authored by Lorenz Halt's avatar Lorenz Halt 🔀
Browse files

WIP: first approach for ilc_rossrv setpoint_generator

parent 8330da59
...@@ -7,6 +7,7 @@ find_package(catkin REQUIRED COMPONENTS ...@@ -7,6 +7,7 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs std_msgs
message_generation message_generation
cppitasc cppitasc
pitasc
pitasc_library pitasc_library
) )
...@@ -35,7 +36,7 @@ generate_messages( ...@@ -35,7 +36,7 @@ generate_messages(
catkin_package( catkin_package(
CATKIN_DEPENDS roscpp cppitasc message_runtime pitasc_library std_msgs CATKIN_DEPENDS roscpp cppitasc message_runtime pitasc_library std_msgs
INCLUDE_DIRS include INCLUDE_DIRS include
LIBRARIES ilc_controllers ilc_scripts LIBRARIES ilc_controllers ilc_scripts ilc_setpoint_generators
# DEPENDS system_lib # DEPENDS system_lib
) )
...@@ -68,10 +69,19 @@ target_link_libraries(ilc_scripts ...@@ -68,10 +69,19 @@ target_link_libraries(ilc_scripts
${catkin_LIBRARIES} ${catkin_LIBRARIES}
) )
add_library(ilc_setpoint_generators
src/ilc/raising_cosine_setpoint_generator_ilc.cpp
)
add_dependencies(ilc_setpoint_generators ${catkin_EXPORTED_TARGETS})
add_dependencies(ilc_setpoint_generators ${${PROJECT_NAME}_EXPORTED_TARGETS})
target_link_libraries(ilc_setpoint_generators
${catkin_LIBRARIES}
)
install(TARGETS install(TARGETS
ilc_controllers ilc_controllers
ilc_scripts ilc_scripts
ilc_setpoint_generators
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
......
...@@ -7,6 +7,7 @@ ...@@ -7,6 +7,7 @@
<!-- <include package="pitasc_library" file="models/skills.xml"/> --> <!-- <include package="pitasc_library" file="models/skills.xml"/> -->
<include package="ilc" file="models/ilc_controllers.xml"/> <include package="ilc" file="models/ilc_controllers.xml"/>
<include package="ilc" file="models/ilc_scripts.xml"/> <include package="ilc" file="models/ilc_scripts.xml"/>
<include package="ilc" file="models/ilc_setpoint_gen.xml"/>
<include package="pitasc_library" file="universal_robots/ur.xml"/> <include package="pitasc_library" file="universal_robots/ur.xml"/>
<!-- <include package="ilc" file="models/skills.xml"/> --> <!-- <include package="ilc" file="models/skills.xml"/> -->
...@@ -64,6 +65,7 @@ ...@@ -64,6 +65,7 @@
<clone id="ilc_skill" prototype="skill_lin"> <clone id="ilc_skill" prototype="skill_lin">
<member id="trajectory_duration">10</member>
<member id="tool_frame">ee_link</member> <member id="tool_frame">ee_link</member>
<member id="target_frame">start_position</member> <member id="target_frame">start_position</member>
...@@ -72,13 +74,32 @@ ...@@ -72,13 +74,32 @@
<member id="tasks.tracking.setpoint_generators"> <member id="tasks.tracking.setpoint_generators">
<clone id="setpoint_cos" prototype="constant_setpoint"> <!-- <clone id="setpoint_cos" prototype="cosine_setpoint_ILC">
<member id="data_source"> <member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/> <reference reference_id="collections.target_to_tool.chains[0]"/>
</member> </member>
<member id="desired" reference_id="target_offsets"/> <member id="desired" reference_id="target_offsets"/>
<member id="coordinates" reference_id="coordinates"/> <member id="coordinates" reference_id="coordinates"/>
<member id="prefix" reference_id="prefix"/> <member id="prefix" reference_id="prefix"/>
<member id="duration" reference_id="trajectory_duration"/>
</clone> -->
<clone id="setpoint_ilc" prototype="cosine_setpoint_ILC">
<member id="data_source">
<reference reference_id="collections.target_to_tool.chains[0]"/>
</member>
<member id="desired" reference_id="target_offsets"/>
<member id="coordinates" reference_id="coordinates"/>
<member id="prefix" reference_id="prefix"/>
<member id="duration" reference_id="trajectory_duration"/>
<member id="max_vel_groups">
<reference reference_id="max_linear_velocity"/>
<reference reference_id="max_angular_velocity"/>
</member>
<member id="symbol_groups">
<clone id="position_coordinates" prototype="string_csv">x, y, z</clone>
<clone id="orientation_coordinates" prototype="string_csv">a, b, c</clone>
</member>
</clone> </clone>
</member> </member>
...@@ -90,7 +111,8 @@ ...@@ -90,7 +111,8 @@
<reference reference_id="collections.target_to_tool.chains[0]"/> <reference reference_id="collections.target_to_tool.chains[0]"/>
</member> </member>
<member id="setpoint_generator"> <member id="setpoint_generator">
<reference reference_id="setpoint_generators.setpoint_cos"/> <reference reference_id="setpoint_generators.setpoint_ilc"/>
<!-- <reference reference_id="setpoint_generators[0]"/> -->
</member> </member>
<member id="coordinates">x, y, z</member> <member id="coordinates">x, y, z</member>
<member id="prefix" reference_id="prefix"/> <member id="prefix" reference_id="prefix"/>
...@@ -106,7 +128,8 @@ ...@@ -106,7 +128,8 @@
<reference reference_id="collections.target_to_tool.chains[0]"/> <reference reference_id="collections.target_to_tool.chains[0]"/>
</member> </member>
<member id="setpoint_generator"> <member id="setpoint_generator">
<reference reference_id="setpoint_generators.setpoint_cos"/> <reference reference_id="setpoint_generators.setpoint_ilc"/>
<!-- <reference reference_id="setpoint_generators[0]"/> -->
</member> </member>
<member id="coordinates">a, b, c</member> <member id="coordinates">a, b, c</member>
<member id="prefix" reference_id="prefix"/> <member id="prefix" reference_id="prefix"/>
......
<?xml version="1.0" encoding="UTF-8"?>
<pitasc>
<models>
<type id="cosine_setpoint_ILC" prototype="setpoint_generator">
<meta>
<member id="description">This setpoint generator calculates a smooth trajectory raising up the final value. The trajectory follows a raising function f=(cos(x+pi)+1)/2 for x in [0, pi]. The duration defines the frequency of the cos. However, the duration is extended accordingly, to respect the maximal velocity at x=pi/2 for each coordinate.</member>
<member id="implementation">
<clone prototype="orocos">
<member id="package">ilc_setpoint_generators</member>
<member id="component">RaisingCosineSetpointGeneratorILC</member>
</clone>
</member>
</meta>
<data>
<type id="duration" prototype="float_parameter">
<meta>
<member id="description">Duration of the smooth setpoint trajectory</member>
</meta>
<data>0</data>
</type>
<type id="symbol_groups" data_type="list:string_csv" prototype="base">
<meta>
<member id="description">List of symbol lists 'symbol_group' that belong to a common movement (e.g. [ [x,y,z],[a,b,c] ]).</member>
</meta>
</type>
<type id="max_vel_groups" data_type="list:float_parameter" prototype="base">
<meta>
<member id="description">Maximum velocity for each symbol_group.</member>
</meta>
</type>
<type id="data_source" data_type="parameter:data_source" prototype="base">
</type>
<type id="coordinates" prototype="string_csv">
</type>
<type id="prefix" prototype="string_parameter">
<meta>
<member id="description">Prefix for the coordinates</member>
</meta>
<data></data>
</type>
<type id="desired" prototype="float_csv">
</type>
</data>
</type>
</models>
</pitasc>
...@@ -18,7 +18,9 @@ class TanHControllerILC : public ILC<TanHController> ...@@ -18,7 +18,9 @@ class TanHControllerILC : public ILC<TanHController>
public: public:
TanHControllerILC(const string& name) TanHControllerILC(const string& name)
: ILC<TanHController>(name) : ILC<TanHController>(name)
{} {
pi_error_named("TanHControllerILC", "Tanh controller adaption for ILC not implemented!");
}
}; };
......
#include <ros/ros.h>
#include <ros/names.h>
#include <std_srvs/Empty.h>
#include <std_srvs/Trigger.h>
#include <std_srvs/SetBool.h>
//#include <string.h>
#include "cppitasc/coordination/setpoint_generator.hpp"
#include <cmath>
using boost::combine;
class RaisingCosineSetpointGeneratorILC : public ConstSetpointGenerator
{
public:
RaisingCosineSetpointGeneratorILC(const string& name)
: ConstSetpointGenerator(name),
started_(false),
duration_(0),
max_duration_(0)
{
}
bool init(Dict& params) override {
pi_debug_named("RaisingCosineSetpointGenerator::init", "component: '{}'", ConstSetpointGenerator::getName());
extract(params["duration"], duration_);
extract(params["max_vel_groups"], max_vel_groups_);
extract(params["symbol_groups"], symbol_groups_);
return ConstSetpointGenerator::init(params);
}
protected:
bool onStart() override {
started_ = false;
distances_start_.assign(desiredCoordinates_.size(), 0);
max_duration_ = 0.0;
return ConstSetpointGenerator::onStart();
}
bool callback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
{
pi_info("Service with name {} called", serviceName_);
called_ = true;
started_ = false;
duration_ = 0.5 * duration_;
return true;
}
bool onConfigure() override {
pi_info("Service with name {} created", "serviceName_");
called_ = false;
serviceName_ = "blub_" + std::to_string(int(duration_));;
service_ = n_.advertiseService(serviceName_, &RaisingCosineSetpointGeneratorILC::callback, this);
return ConstSetpointGenerator::onConfigure();
}
void onCleanup() override {
pi_info("Service with name {} shut down", serviceName_);
called_ = false;
service_.shutdown();
ConstSetpointGenerator::onCleanup();
}
void onUpdate(const Tick& tick) final {
if (called_) {
called_ = false;
pi_warn("CALLED: {}", duration_);
pi_error("CALLED: {}", duration_);
pi_warn("CALLED: {}", duration_);
}
// Scaling on actual first measured value as being the current starting point
// begin initialization
if (!started_) {
unsigned int critical_idx;
string critical_coordinate;
auto measuredValues = dataSource_->getData();
// desiredValues_ is constant and defined in base class
for (auto zip: combine(distances_start_, desiredCoordinates_, desiredValues_)) {
boost::get<0>(zip) = measuredValues[boost::get<1>(zip)];
auto distance = boost::get<2>(zip) - boost::get<0>(zip);
// get index of corresponding symbol group
string c = boost::get<1>(zip);
c.erase(0, c.find_last_of("/")+1);
auto idx = getIndexOfSymbolGroup(c, symbol_groups_);
/* Deduction of the duration formula:
* The setpoint trajectory follows the function f(t) = -s*(cos(pi * t/d)-1)/2)
* with s: the final setpoint, d: the duration until s is reached
*
* The velocity derives to df/dt = f' = pi*s*sin(pi * t/d) / (2*d)
* The velocity reaches its maximum d^2f/dt^2 = f'' = pi^2*s*cos(pi * t/d ) / (2*d^2) = 0
* at f''(t=d/2) = 0
*
* The maximal velocity calculates to v_max = f'(t=d/2) = pi*s / (2*d),
* thus the duration d with the (limited) velocity v_max
* d = pi*s/(2*v_max) = pi/2 * s / v_max
*/
auto duration = ( (M_PI_2 * std::abs(distance)) / max_vel_groups_[idx] );
/* If any duration exceeds the parametrized duration, the velocity exceeds its limit
* The longest duration (with given velocity limitation) defines the length of the overall setpoint trajectory
*/
if (duration > max_duration_) {
max_duration_ = duration;
critical_idx = idx;
critical_coordinate = c;
}
}
start_ = tick;
started_ = true;
// Output some messages to see what is goint on
// max_vel is stronger than duration
if (max_duration_ > duration_) {
if (duration_ > 0) {
pi_warn_named("RaisingCosineSetpointGenerator::onUpdate",
"The duration was set to {}s, to obey velocity limitation of symbol {} ({}units/s).",
max_duration_, critical_coordinate, max_vel_groups_[critical_idx]);
} else {
// Avoid warn message if duration was set to "as fast as possible" on purpose
pi_debug_named("RaisingCosineSetpointGenerator::onUpdate",
"The duration was set to {}s, to obey velocity limitation of symbol {} ({}units/s).",
max_duration_, critical_coordinate, max_vel_groups_[critical_idx]);
}
} else {
max_duration_ = duration_;
pi_debug_named("RaisingCosineSetpointGenerator::onUpdate",
"The duration is {}s, and within all velocity limitation.",
duration_);
}
}
// End initialization
// Actual setpoint generation
for (auto zip: combine(desiredCoordinates_, distances_start_, desiredValues_)) {
std::chrono::duration<double> t_start = tick - start_;
data_[boost::get<0>(zip)] = adjust(boost::get<1>(zip),
boost::get<2>(zip),
cosTrajectory(
max_duration_,
t_start.count()
)
);
}
}
/* cosine trajectory without scaling by initial error distance nor offset with desired value
* trajectory only depends on t_end and can be used for each controlled variable (with respective scaling)
*
* The value of the cosine should be in [0, 1] (adding 1 and dividing by 2) and should be exactly one "rising edge", so the angle is M_PI
*/
double cosTrajectory( const double& duration,
double t)
{
// Cutting off cosine outside of [0, duration] to have only a raising edge
if (t > duration) {
return 0;
} else if (t < 0.0) {
return 1;
} else {
return (cos(M_PI * t / duration) + 1.0) / 2.0;
}
}
// scaling of trajectory value v, to be between y_start and y_end
double adjust( const double& y_start,
const double& y_end,
double y)
{
return (y * (y_start - y_end)) + y_end;
}
unsigned int getIndexOfSymbolGroup( const string& symbol, const vector<vector<string>>& symbol_groups) {
unsigned int idx = 0;
for (auto group: symbol_groups) {
if (std::find(group.begin(), group.end(), symbol) != group.end()){
// Element in vector.
return idx;
} else {
idx++;
}
}
pi_error("Symbol '{}' not found in any_symbol_group!", symbol);
throw std::invalid_argument("Symbol '"+ symbol +"' not found in any_symbol_group!");
}
private:
Tick start_;
bool started_;
vector<double> distances_start_;
double duration_;
double max_duration_;
vector<double> max_vel_groups_;
vector<vector<string>> symbol_groups_;
ros::NodeHandle n_;
ros::ServiceServer service_;
bool called_;
protected:
string serviceName_;
};
RUNTIME_COMPONENT(RaisingCosineSetpointGeneratorILC)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment