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

Retime service model

parent da605c4e
......@@ -17,6 +17,7 @@ find_package(Eigen REQUIRED)
add_service_files(
FILES
TriggerILC.srv
RetimeILC.srv
)
## Uncomment this if the package has a setup.py. This macro ensures
......@@ -55,7 +56,7 @@ target_link_libraries(ilc_controllers
add_library(ilc_scripts
src/ilc/script_trigger_ilc.cpp
src/ilc/scripts_ilc.cpp
)
target_link_libraries(ilc_scripts
${catkin_LIBRARIES}
......
<?xml version="1.0" encoding="UTF-8"?>
<pitasc>
<models>
<include package="pitasc_library" file="models/pitasc.xml"/>
<!-- <include package="pitasc_library" file="models/skills.xml"/> -->
<include package="ilc" file="models/ilc_scripts.xml"/>
<include package="ilc" file="models/ilc_controllers.xml"/>
<include package="pitasc_library" file="universal_robots/ur.xml"/>
<!-- <include package="ilc" file="models/skills.xml"/> -->
</models>
<!-- Create a project -->
<clone prototype="project">
<member id="configuration">
<!-- Use the default configuration with recommended settings -->
<clone id="configuration" prototype="default_configuration"/>
</member>
<member id="environment">
<!-- Add a UR5 -->
<clone prototype="robot_ur5">
<member id="robot_driver.max_velocity">10</member>
<member id="robot_driver.max_acceleration">10</member>
<member id="components">
<clone prototype="force_sensor">
<member id="wrench_topic">wrench</member>
</clone>
</member>
</clone>
</member>
<member id="applications">
<!-- Use a skill sequence as a container -->
<clone prototype="skill_sequence">
<!-- Use the UR5 for this skill (and for its subskill) -->
<member id="robot" reference_id="environment.robot_ur5"/>
<!-- Add subskill(s) -->
<member id="skills">
<clone prototype="skill_lin">
<member id="tool_frame">ee_link</member>
<member id="target_frame">start_position</member>
<member id="scripts">
<clone prototype="script_retime_ilc">
<member id="service_name">ilc_retime</member>
<member id="setpoint_duration_ms">100</member>
<member id="reuse_ff">True</member>
<member id="reuse_scheme">0</member>
</clone>
</member>
</clone>
</member>
</clone>
</member>
</clone>
</pitasc>
......@@ -11,7 +11,6 @@
<clone prototype="orocos">
<member id="package">ilc_scripts</member>
<member id="component">ScriptTriggerILC</member>
<member id="arguments">on_start, namespace, service_name, wait_for_service, file_in, file_out, file_traj, traj_type, L, cutoff_freq</member>
</clone>
</member>
</meta>
......@@ -55,6 +54,33 @@
</data>
</type>
<type id="script_retime_ilc" prototype="script_service_caller">
<meta>
<member id="description">Commands a ilc_proxy to change the duration of the setpoint trajectory to the given length.</member>
<member id="implementation">
<clone prototype="orocos">
<member id="package">ilc_scripts</member>
<member id="component">ScriptRetimeILC</member>
</clone>
</member>
</meta>
<data>
<type id="setpoint_duration_ms" prototype="int_parameter">
<member id="description">Wanted duration in milliseconds</member>
</type>
<type id="reuse_ff" prototype="bool_parameter">
<member id="description">Reshape and reuse previous learned feedforwards</member>
<data>True</data>
</type>
<type id="reuse_scheme" prototype="int_parameter">
<member id="description">Selects different reshaping schemes. 0: No reshaping</member>
<data>0</data>
</type>
</data>
</type>
</models>
</pitasc>
#!/usr/bin/env python
from __future__ import print_function
from ilc.srv import TriggerILC, TriggerILCResponse
from ilc.srv import TriggerILC, TriggerILCResponse, RetimeILC, RetimeILCResponse
import rospy
import numpy as np
import ilc_lib
......@@ -17,7 +17,9 @@ class Ilc_serviceproxy:
self.T = rospy.get_param('~T', 8) # sampling time in ms
self.fs = 1.0/(self.T * 0.001) # sampling frequency
self.service = rospy.Service('ilc_service', TriggerILC, self.callback_ilc_calc)
rospy.Service('ilc_service', TriggerILC, self.callback_ilc_calc)
rospy.Service('ilc_retime', RetimeILC, self.callback_ilc_retime)
self.folder = rospy.get_param('~folder', None)
def callback_ilc_calc(self, req):
......@@ -71,6 +73,10 @@ class Ilc_serviceproxy:
message = str(e)
return TriggerILCResponse(success=False, message=message)
def callback_ilc_retime(self, req):
print(req)
#self.decay_time = req.setpoint_duration_ms
return RetimeILCResponse(success=True, message="done")
if __name__ == '__main__':
......
#include "cppitasc/coordination/script_rosservice.hpp"
#include <ilc/TriggerILC.h>
#include <ilc/RetimeILC.h>
class ScriptTriggerILC : public ScriptRosService<ilc::TriggerILC, ilc::TriggerILC::Request, ilc::TriggerILC::Response>
{
......@@ -48,4 +49,40 @@ private:
};
class ScriptRetimeILC : public ScriptRosService<ilc::RetimeILC, ilc::RetimeILC::Request, ilc::RetimeILC::Response>
{
public:
ScriptRetimeILC(const string& name)
: ScriptRosService<ilc::RetimeILC, ilc::RetimeILC::Request, ilc::RetimeILC::Response>(
name)
{
}
bool init(Dict& params) override {
extract(params["setpoint_duration_ms"], setpoint_duration_ms_);
extract(params["reuse_ff"], reuse_ff_);
extract(params["reuse_scheme"], reuse_scheme_);
service_.request.setpoint_duration_ms = setpoint_duration_ms_;
service_.request.reuse_ff = reuse_ff_;
service_.request.reuse_scheme = reuse_scheme_;
return ScriptRosService::init(params);
}
bool call() override {
bool result = ScriptRosService<ilc::RetimeILC, ilc::RetimeILC::Request, ilc::RetimeILC::Response>::call();
if (!result) {
pi_error("Message: {}", service_.response.message);
}
return service_.response.success;
}
private:
int setpoint_duration_ms_;
bool reuse_ff_;
int reuse_scheme_;
};
RUNTIME_COMPONENT(ScriptTriggerILC)
RUNTIME_COMPONENT(ScriptRetimeILC)
uint8 setpoint_duration_ms
bool reuse_ff
uint8 reuse_scheme
----------------------
bool success
string message
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