Skip to content

Commit 30c010f

Browse files
author
Yoan Mollard
committed
Initial commit
0 parents  commit 30c010f

File tree

9 files changed

+913
-0
lines changed

9 files changed

+913
-0
lines changed

‎CMakeLists.txt‎

Lines changed: 156 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,156 @@
1+
cmake_minimum_required(VERSION 2.8.3)
2+
project(dmp_lib)
3+
4+
## Find catkin macros and libraries
5+
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
6+
## is used, also find other catkin packages
7+
find_package(catkin REQUIRED)
8+
9+
## System dependencies are found with CMake's conventions
10+
# find_package(Boost REQUIRED COMPONENTS system)
11+
12+
13+
## Uncomment this if the package has a setup.py. This macro ensures
14+
## modules and global scripts declared therein get installed
15+
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
16+
catkin_python_setup()
17+
18+
################################################
19+
## Declare ROS messages, services and actions ##
20+
################################################
21+
22+
## To declare and build messages, services or actions from within this
23+
## package, follow these steps:
24+
## * Let MSG_DEP_SET be the set of packages whose message types you use in
25+
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
26+
## * In the file package.xml:
27+
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
28+
## * If MSG_DEP_SET isn't empty the following dependencies might have been
29+
## pulled in transitively but can be declared for certainty nonetheless:
30+
## * add a build_depend tag for "message_generation"
31+
## * add a run_depend tag for "message_runtime"
32+
## * In this file (CMakeLists.txt):
33+
## * add "message_generation" and every package in MSG_DEP_SET to
34+
## find_package(catkin REQUIRED COMPONENTS ...)
35+
## * add "message_runtime" and every package in MSG_DEP_SET to
36+
## catkin_package(CATKIN_DEPENDS ...)
37+
## * uncomment the add_*_files sections below as needed
38+
## and list every .msg/.srv/.action file to be processed
39+
## * uncomment the generate_messages entry below
40+
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
41+
42+
## Generate messages in the 'msg' folder
43+
# add_message_files(
44+
# FILES
45+
# Message1.msg
46+
# Message2.msg
47+
# )
48+
49+
## Generate services in the 'srv' folder
50+
# add_service_files(
51+
# FILES
52+
# Service1.srv
53+
# Service2.srv
54+
# )
55+
56+
## Generate actions in the 'action' folder
57+
# add_action_files(
58+
# FILES
59+
# Action1.action
60+
# Action2.action
61+
# )
62+
63+
## Generate added messages and services with any dependencies listed here
64+
# generate_messages(
65+
# DEPENDENCIES
66+
# std_msgs # Or other packages containing msgs
67+
# )
68+
69+
###################################
70+
## catkin specific configuration ##
71+
###################################
72+
## The catkin_package macro generates cmake config files for your package
73+
## Declare things to be passed to dependent projects
74+
## INCLUDE_DIRS: uncomment this if you package contains header files
75+
## LIBRARIES: libraries you create in this project that dependent projects also need
76+
## CATKIN_DEPENDS: catkin_packages dependent projects also need
77+
## DEPENDS: system dependencies of this project that dependent projects also need
78+
catkin_package(
79+
# INCLUDE_DIRS include
80+
# LIBRARIES dmp_lib
81+
# CATKIN_DEPENDS other_catkin_pkg
82+
# DEPENDS system_lib
83+
)
84+
85+
###########
86+
## Build ##
87+
###########
88+
89+
## Specify additional locations of header files
90+
## Your package locations should be listed before other locations
91+
# include_directories(include)
92+
93+
## Declare a cpp library
94+
# add_library(dmp_lib
95+
# src/${PROJECT_NAME}/dmp_lib.cpp
96+
# )
97+
98+
## Declare a cpp executable
99+
# add_executable(dmp_lib_node src/dmp_lib_node.cpp)
100+
101+
## Add cmake target dependencies of the executable/library
102+
## as an example, message headers may need to be generated before nodes
103+
# add_dependencies(dmp_lib_node dmp_lib_generate_messages_cpp)
104+
105+
## Specify libraries to link a library or executable target against
106+
# target_link_libraries(dmp_lib_node
107+
# ${catkin_LIBRARIES}
108+
# )
109+
110+
#############
111+
## Install ##
112+
#############
113+
114+
# all install targets should use catkin DESTINATION variables
115+
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
116+
117+
## Mark executable scripts (Python etc.) for installation
118+
## in contrast to setup.py, you can choose the destination
119+
install(PROGRAMS
120+
bin/dmp_discrete
121+
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
122+
)
123+
124+
## Mark executables and/or libraries for installation
125+
# install(TARGETS dmp_lib dmp_lib_node
126+
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
127+
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
128+
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
129+
# )
130+
131+
## Mark cpp header files for installation
132+
# install(DIRECTORY include/${PROJECT_NAME}/
133+
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
134+
# FILES_MATCHING PATTERN "*.h"
135+
# PATTERN ".svn" EXCLUDE
136+
# )
137+
138+
## Mark other files for installation (e.g. launch and bag files, etc.)
139+
# install(FILES
140+
# # myfile1
141+
# # myfile2
142+
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
143+
# )
144+
145+
#############
146+
## Testing ##
147+
#############
148+
149+
## Add gtest based cpp test target and link libraries
150+
# catkin_add_gtest(${PROJECT_NAME}-test test/test_dmp_lib.cpp)
151+
# if(TARGET ${PROJECT_NAME}-test)
152+
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
153+
# endif()
154+
155+
## Add folders to be run by python nosetests
156+
# catkin_add_nosetests(test)

‎package.xml‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
<?xml version="1.0"?>
2+
<package>
3+
<name>dmp_lib</name>
4+
<version>0.0.0</version>
5+
<description>DMP generator</description>
6+
7+
<maintainer email="[email protected]">Busch Baptiste</maintainer>
8+
<license>TODO</license>
9+
10+
<!-- The *_depend tags are used to specify dependencies -->
11+
<!-- Dependencies can be catkin packages or system dependencies -->
12+
<!-- Examples: -->
13+
<!-- Use build_depend for packages you need at compile time: -->
14+
<!-- <build_depend>message_generation</build_depend> -->
15+
<!-- Use buildtool_depend for build tool packages: -->
16+
<!-- <buildtool_depend>catkin</buildtool_depend> -->
17+
<!-- Use run_depend for packages you need at runtime: -->
18+
<!-- <run_depend>message_runtime</run_depend> -->
19+
<!-- Use test_depend for packages you need only for testing: -->
20+
<!-- <test_depend>gtest</test_depend> -->
21+
<buildtool_depend>catkin</buildtool_depend>
22+
23+
24+
<!-- The export tag contains other, unspecified, tags -->
25+
<export>
26+
<!-- You can specify that this package is a metapackage here: -->
27+
<!-- <metapackage/> -->
28+
29+
<!-- Other tools can request additional information be placed here -->
30+
31+
</export>
32+
</package>

‎scripts/dmp_discrete‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
#! /usr/bin/env python
2+
3+
import dmp_lib.dmp_discrete
4+
5+
if __name__ == '__main__':
6+
dmp_lib.dmp_discrete.test()

‎setup.py‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
2+
3+
from distutils.core import setup
4+
from catkin_pkg.python_setup import generate_distutils_setup
5+
6+
# fetch values from package.xml
7+
setup_args = generate_distutils_setup(
8+
packages=['dmp_lib'],
9+
package_dir={'': 'src'},
10+
)
11+
12+
setup(**setup_args)

‎src/dmp_lib/__init__.py‎

Whitespace-only changes.

‎src/dmp_lib/cs.py‎

Lines changed: 143 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,143 @@
1+
'''
2+
Copyright (C) 2013 Travis DeWolf
3+
4+
This program is free software: you can redistribute it and/or modify
5+
it under the terms of the GNU General Public License as published by
6+
the Free Software Foundation, either version 3 of the License, or
7+
(at your option) any later version.
8+
9+
This program is distributed in the hope that it will be useful,
10+
but WITHOUT ANY WARRANTY; without even the implied warranty of
11+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12+
GNU General Public License for more details.
13+
14+
You should have received a copy of the GNU General Public License
15+
along with this program. If not, see <http://www.gnu.org/licenses/>.
16+
'''
17+
18+
import numpy as np
19+
20+
class CanonicalSystem():
21+
"""Implementation of the canonical dynamical system
22+
as described in Dr. Stefan Schaal's (2002) paper"""
23+
24+
def __init__(self, dt, pattern='discrete'):
25+
"""Default values from Schaal (2012)
26+
27+
dt float: the timestep
28+
pattern string: either 'discrete' or 'rhythmic'
29+
"""
30+
self.ax = 1.0
31+
32+
self.pattern = pattern
33+
if pattern == 'discrete':
34+
self.step = self.step_discrete
35+
self.run_time = 1.0
36+
elif pattern == 'rhythmic':
37+
self.step = self.step_rhythmic
38+
self.run_time = 2*np.pi
39+
else:
40+
raise Exception('Invalid pattern type specified: \
41+
Please specify rhythmic or discrete.')
42+
43+
self.dt = dt
44+
self.timesteps = int(self.run_time / self.dt)
45+
46+
self.reset_state()
47+
48+
def rollout(self, **kwargs):
49+
"""Generate x for open loop movements.
50+
"""
51+
if kwargs.has_key('tau'):
52+
timesteps = int(self.timesteps / kwargs['tau'])
53+
else:
54+
timesteps = self.timesteps
55+
self.x_track = np.zeros(timesteps)
56+
57+
self.reset_state()
58+
for t in range(timesteps):
59+
self.x_track[t] = self.x
60+
self.step(**kwargs)
61+
62+
return self.x_track
63+
64+
def reset_state(self):
65+
"""Reset the system state"""
66+
self.x = 1.0
67+
68+
def step_discrete(self, tau=1.0, error_coupling=1.0):
69+
"""Generate a single step of x for discrete
70+
(potentially closed) loop movements.
71+
Decaying from 1 to 0 according to dx = -ax*x.
72+
73+
tau float: gain on execution time
74+
increase tau to make the system execute faster
75+
error_coupling float: slow down if the error is > 1
76+
"""
77+
self.x += (-self.ax * self.x * error_coupling) * tau * self.dt
78+
return self.x
79+
80+
def step_rhythmic(self, tau=1.0, error_coupling=1.0):
81+
"""Generate a single step of x for rhythmic
82+
closed loop movements. Decaying from 1 to 0
83+
according to dx = -ax*x.
84+
85+
tau float: gain on execution time
86+
increase tau to make the system execute faster
87+
error_coupling float: slow down if the error is > 1
88+
"""
89+
self.x += (1 * error_coupling * tau) * self.dt
90+
return self.x
91+
92+
93+
#==============================
94+
# Test code
95+
#==============================
96+
if __name__ == "__main__":
97+
98+
cs = CanonicalSystem(dt=.001, pattern='discrete')
99+
# test normal rollout
100+
x_track1 = cs.rollout()
101+
102+
cs.reset_state()
103+
# test error coupling
104+
timesteps = int(1.0/.001)
105+
x_track2 = np.zeros(timesteps)
106+
err = np.zeros(timesteps)
107+
err[200:400] = 2
108+
err_coup = 1.0 / (1 + err)
109+
for i in range(timesteps):
110+
x_track2[i] = cs.step(error_coupling=err_coup[i])
111+
112+
import matplotlib.pyplot as plt
113+
fig, ax1 = plt.subplots(figsize=(6,3))
114+
ax1.plot(x_track1, lw=2)
115+
ax1.plot(x_track2, lw=2)
116+
plt.grid()
117+
plt.legend(['normal rollout', 'error coupling'])
118+
ax2 = ax1.twinx()
119+
ax2.plot(err, 'r-', lw=2)
120+
plt.legend(['error'], loc='lower right')
121+
plt.ylim(0, 3.5)
122+
plt.xlabel('time (s)')
123+
plt.ylabel('x')
124+
plt.title('Canonical system - discrete')
125+
126+
for t1 in ax2.get_yticklabels():
127+
t1.set_color('r')
128+
129+
plt.tight_layout()
130+
131+
cs = CanonicalSystem(dt=.001, pattern='rhythmic')
132+
# test normal rollout
133+
x_track1 = cs.rollout()
134+
135+
import matplotlib.pyplot as plt
136+
fig, ax1 = plt.subplots(figsize=(6,3))
137+
ax1.plot(x_track1, lw=2)
138+
plt.grid()
139+
plt.legend(['normal rollout'], loc='lower right')
140+
plt.xlabel('time (s)')
141+
plt.ylabel('x')
142+
plt.title('Canonical system - rhythmic')
143+
plt.show()

0 commit comments

Comments
 (0)