From 6a9012c6e0382416a4757a7372c1083fd1ddd1aa Mon Sep 17 00:00:00 2001 From: iory Date: Wed, 14 Mar 2018 17:13:33 +0900 Subject: [PATCH 1/2] Add interpolator --- robot/__init__.py | 3 +- robot/interpolator.py | 242 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 244 insertions(+), 1 deletion(-) create mode 100644 robot/interpolator.py diff --git a/robot/__init__.py b/robot/__init__.py index 5acbd9b3..c56bbded 100644 --- a/robot/__init__.py +++ b/robot/__init__.py @@ -2,11 +2,12 @@ worldcoords = coordinates.CascadedCoords() +from robot import interpolator from robot import math from robot import optimizer from robot import optimizers from robot import robot_model from robot import utils -from robot.utils.urdf import URDF from robot.misc import listify # NOQA from robot.pybullet_robot_interface import PybulletRobotInterface +from robot.utils.urdf import URDF diff --git a/robot/interpolator.py b/robot/interpolator.py new file mode 100644 index 00000000..c08aadeb --- /dev/null +++ b/robot/interpolator.py @@ -0,0 +1,242 @@ +import numpy as np + + +class Interpolator(object): + + def __init__(self): + self.time = 0.0 + self.segment_time = 0.0 + self.segment = 0 + self.segment_num = 0 + self.is_interpolating = False + + def reset(self, *args, + position_list=None, + time_list=None): + """"Initialize interpolator + + Args: + position-list: + list of control point + time-list: + list of time from start for each control point, + time in fisrt contrall point is zero, so length of this list is length of control point minus 1 + + """ + if position_list is None: + position_list = self.position_list + else: + self.position_list = position_list + if time_list is None: + time_list = self.time_list + else: + self.time_list = time_list + if len(position_list) != len(time_list) + 1: + raise ValueError("length of position_list must be length of time_list + 1") + + self.time = 0.0 + self.segment_time = 0.0 + self.segment = 0 + self.segment_num = len(position_list) - 1 + self.stop_interpolation() + + def start_interpolation(self): + self.is_interpolating = True + + def stop_interpolation(self): + self.is_interpolating = False + + def interpolation(self): + raise NotImplementedError + + def pass_time(self, dt): + """process interpolation for dt[sec] + + Args: + dt (float): + sec order + """ + if self.is_interpolating: + self.position = self.interpolation() + self.time += dt + self.segment_time += dt + if self.time > self.time_list[self.segment]: + self.segment_time = (self.time - self.time_list[self.segment]) + self.segment += 1 + if self.segment >= self.segment_num: + self.reset() + return self.position + + +class LinearInterpolator(Interpolator): + + def __init__(self): + Interpolator.__init__(self) + + def interpolation(self): + """Linear Interpolation""" + v1 = self.position_list[self.segment] + v2 = self.position_list[self.segment + 1] + if self.segment > 0: + total_time = self.time_list[self.segment] - self.time_list[self.segment - 1] + else: + total_time = self.time_list[self.segment] + t1 = self.segment_time + t2 = total_time - t1 + v1 = v1 * (t2 / total_time) + v2 = v2 * (t1 / total_time) + return v1 + v2 + + +class MinjerkInterpolator(Interpolator): + + def __init__(self): + Interpolator.__init__(self) + + def reset(self, *args, + velocity_list=None, + acceleration_list=None, + **kwargs): + """ + + Args: + position_list: + list of control point + velocity-list: + list of velocity in each control point + acceleration-list: + list of acceleration in each control point + + """ + Interpolator.reset(self, *args, **kwargs) + if velocity_list is None: + self.velocity_list = [np.zeros(len(self.position_list[0])) for _ in range(self.segment_num + 1)] + else: + self.velocity_list = velocity_list + if acceleration_list is None: + self.acceleration_list = [np.zeros(len(self.position_list[0])) for _ in range(self.segment_num + 1)] + else: + self.acceleration_list = acceleration_list + + def interpolation(self): + """Minjerk interpolator, a.k.a Hoff & Arbib + + """ + xi = self.position_list[self.segment] + xf = self.position_list[self.segment + 1] + vi = self.velocity_list[self.segment] + vf = self.velocity_list[self.segment + 1] + ai = self.acceleration_list[self.segment] + af = self.acceleration_list[self.segment + 1] + if self.segment > 0: + total_time = self.time_list[self.segment] - self.time_list[self.segment - 1] + else: + total_time = self.time_list[self.segment] + + # A=(gx-(x+v*t+(a/2.0)*t*t))/(t*t*t) + # B=(gv-(v+a*t))/(t*t) + # C=(ga-a)/toi + + A = (xf - (xi + total_time * vi + (total_time ** 2) * 0.5 * ai)) / (total_time ** 3) + B = (vf - (vi + total_time * ai)) / (total_time ** 2) + C = (af - ai) / total_time + + # a0=x + # a1=v + # a2=a/2.0 + # a3=10*A-4*B+0.5*C + # a4=(-15*A+7*B-C)/t + # a5=(6*A-3*B+0.5*C)/(t*t) + + a0 = xi + a1 = vi + a2 = 0.5 * ai + a3 = 10 * A - 4 * B + 0.5 * C + a4 = (-15 * A + 7 * B - C) / total_time + a5 = (6 * A - 3 * B + 0.5 * C) / (total_time * total_time) + + # x=a0+a1*t+a2*t*t+a3*t*t*t+a4*t*t*t*t+a5*t*t*t*t*t + # v=a1+2*a2*t+3*a3*t*t+4*a4*t*t*t+5*a5*t*t*t*t + # a=2*a2+6*a3*t+12*a4*t*t+20*a5*t*t*t + + self.position = a0 + \ + self.segment_time ** 1 * a1 + \ + self.segment_time ** 2 * a2 + \ + self.segment_time ** 3 * a3 + \ + self.segment_time ** 4 * a4 + \ + self.segment_time ** 5 * a5 + + self.velocity = a1 + \ + self.segment_time ** 1 * a2 + \ + self.segment_time ** 2 * a3 + \ + self.segment_time ** 3 * a4 + \ + self.segment_time ** 4 * a5 + + self.acceleration = a2 + \ + self.segment_time ** 1 * a3 + \ + self.segment_time ** 2 * a4 + \ + self.segment_time ** 3 * a5 + return self.position + + +def position_list_interpolation( + position_list, time_list, dt, + interpolator=MinjerkInterpolator(), + initial_time=0.0, + neglect_first=False, + vel_vector_list=None, + acc_vector_list=None): + data_list = [] + tm_list = [] + vel_data_list = [] + acc_data_list = [] + + if vel_vector_list is None: + vel_vector_list = [] + if acc_vector_list is None: + acc_vector_list = [] + + r = [] + for n in time_list: + if len(r): + r.append(n + r[-1]) + else: + r.append(n) + kwargs = dict(position_list=position_list, + time_list=r) + if hasattr(interpolator, 'velocity'): + kwargs['velocity_list'] = vel_vector_list + if hasattr(interpolator, 'acceleration'): + kwargs['acceleration_list'] = acc_vector_list + interpolator.reset(**kwargs) + interpolator.start_interpolation() + while interpolator.is_interpolating: + if interpolator.is_interpolating: + tm_list.append(initial_time + interpolator.time) + else: + tm_list.append(dt + tm_list[0]) + interpolator.pass_time(dt) + data_list.append(interpolator.position) + + if hasattr(interpolator, 'velocity'): + vel_data_list.append(interpolator.velocity) + if hasattr(interpolator, 'acceleration'): + acc_data_list.append(interpolator.acceleration) + + if neglect_first: + data_list = data_list[1:] + tm_list = tm_list[1:] + + result_dict = dict(position=data_list, + time=tm_list) + if hasattr(interpolator, 'velocity'): + if neglect_first: + result_dict['velocity'] = vel_data_list[1:] + else: + result_dict['velocity'] = vel_data_list + if hasattr(interpolator, 'acceleration'): + if neglect_first: + result_dict['acceleration'] = acc_data_list[1:] + else: + result_dict['acceleration'] = acc_data_list + return result_dict From 7f44095a01d3c6eee979926049a8ce6f2c26d358 Mon Sep 17 00:00:00 2001 From: iory Date: Sun, 18 Mar 2018 13:31:35 +0900 Subject: [PATCH 2/2] [test] Add interpolator test --- tests/robot_tests/test_interpolator.py | 62 ++++++++++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 tests/robot_tests/test_interpolator.py diff --git a/tests/robot_tests/test_interpolator.py b/tests/robot_tests/test_interpolator.py new file mode 100644 index 00000000..7eed237c --- /dev/null +++ b/tests/robot_tests/test_interpolator.py @@ -0,0 +1,62 @@ +import unittest + +from numpy import pi +from numpy import testing +import numpy as np + +from robot.interpolator import LinearInterpolator +from robot.interpolator import MinjerkInterpolator +from robot.math import midpoint + + +class TestInterpolator(unittest.TestCase): + + def test_linear_interpolator(self): + ip = LinearInterpolator() + p0 = np.array([1, 2, 3]) + t0 = 0.10 + p1 = np.array([3, 4, 5]) + t1 = 0.18 + ip.reset(position_list=[p0, p1, p0], + time_list=[t0, t1]) + ip.start_interpolation() + + i = 0 + while t0 > i: + ip.pass_time(0.02) + testing.assert_almost_equal(ip.position, midpoint(i / t0, p0, p1)) + i += 0.02 + + i = t0 + while t1 > i: + ip.pass_time(0.02) + testing.assert_almost_equal( + ip.position, + midpoint((i - t0) / (t1 - t0), p1, p0)) + i += 0.02 + + assert(ip.is_interpolating is False) + + def test_minjerk_interpolator(self): + ip = MinjerkInterpolator() + p0 = np.array([1, 2, 3]) + t0 = 0.10 + p1 = np.array([3, 4, 5]) + t1 = 0.18 + ip.reset(position_list=[p0, p1, p0], + time_list=[t0, t1]) + ip.start_interpolation() + + i = 0 + while t0 >= i: + ip.pass_time(0.02) + i += 0.02 + testing.assert_almost_equal(ip.position, p1) + + i = t0 + while t1 >= i: + ip.pass_time(0.02) + i += 0.02 + testing.assert_almost_equal(ip.position, p0) + + assert(ip.is_interpolating is False)