#
# MIT License
#
# Copyright (c) 2020-2021 NVIDIA CORPORATION.
#
# Permission is hereby granted, free of charge, to any person obtaining a
# copy of this software and associated documentation files (the "Software"),
# to deal in the Software without restriction, including without limitation
# the rights to use, copy, modify, merge, publish, distribute, sublicense,
# and/or sell copies of the Software, and to permit persons to whom the
# Software is furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
# DEALINGS IN THE SOFTWARE.#
import numpy as np
import copy
[docs]class AlphaBetaFilter(object):
def __init__(self, filter_coeff=0.4):
self.raw_state = None
self.filter_coeff = filter_coeff
[docs] def filter(self, raw_state):
self.raw_state = (1 - self.filter_coeff) * self.raw_state + self.filter_coeff * raw_state
[docs] def two_state_filter(self, raw_state1, raw_state2):
new_state = self.filter_coeff * raw_state1 + (1 - self.filter_coeff) * raw_state2
return new_state
[docs]class RobotStateFilter(object):
def __init__(self, filter_keys=['position', 'velocity','acceleration'], filter_coeff={'position':0.1, 'velocity':0.1,'acceleration':0.1},dt=0.1):
self.prev_filtered_state = None
self.filtered_state = None
self.filter_coeff = filter_coeff
self.filter_keys = filter_keys
self.dt = dt
[docs] def filter_state(self, raw_state, dt=None):
dt = self.dt if dt is None else dt
if(self.filtered_state is None):
self.filtered_state = copy.deepcopy(raw_state)
if('acceleration' in self.filter_keys):
self.filtered_state['acceleration'] = 0.0* raw_state['position']
#return self.filtered_state
self.prev_filtered_state = copy.deepcopy(self.filtered_state)
for k in self.filter_keys:
if(k in raw_state.keys()):
self.filtered_state[k] = self.filter_coeff[k] * raw_state[k] + (1.0 - self.filter_coeff[k]) * self.filtered_state[k]
if('acceleration' in self.filter_keys):# and 'acceleration' not in raw_state):
self.filtered_state['acceleration'] = (self.filtered_state['velocity'] - self.prev_filtered_state['velocity']) / dt
return self.filtered_state
[docs]class JointStateFilter(object):
def __init__(self, raw_joint_state=None, filter_coeff=0.4, dt=0.1, filter_keys=['position','velocity','acceleration']):
self.cmd_joint_state = copy.deepcopy(raw_joint_state)
self.filter_coeff = {}
if not isinstance(filter_coeff,dict):
for k in filter_keys:
self.filter_coeff[k] = filter_coeff
else:
self.filter_coeff = filter_coeff
self.dt = dt
self.filter_keys = filter_keys
self.prev_cmd_qdd = None
[docs] def filter_joint_state(self, raw_joint_state):
if(self.cmd_joint_state is None):
self.cmd_joint_state = copy.deepcopy(raw_joint_state)
return self.cmd_joint_state
for k in self.filter_keys:
self.cmd_joint_state[k] = self.filter_coeff[k] * raw_joint_state[k] + (1.0 - self.filter_coeff[k]) * self.cmd_joint_state[k]
return self.cmd_joint_state
[docs] def forward_predict_internal_state(self, dt=None):
if(self.prev_cmd_qdd is None):
return
dt = self.dt if dt is None else dt
self.cmd_joint_state['acceleration'] = self.prev_cmd_qdd
self.cmd_joint_state['velocity'] = self.cmd_joint_state['velocity'] + self.prev_cmd_qdd * dt
self.cmd_joint_state['position'] = self.cmd_joint_state['position'] + self.cmd_joint_state['velocity'] * dt
[docs] def predict_internal_state(self, qdd_des=None, dt=None):
if(qdd_des is None):
return
dt = self.dt if dt is None else dt
self.cmd_joint_state['acceleration'] = qdd_des
self.cmd_joint_state['velocity'] = self.cmd_joint_state['velocity'] + qdd_des * dt
self.cmd_joint_state['position'] = self.cmd_joint_state['position'] + self.cmd_joint_state['velocity'] * dt
[docs] def integrate_jerk(self, qddd_des, raw_joint_state, dt=None):
dt = self.dt if dt is None else dt
self.filter_joint_state(raw_joint_state)
self.cmd_joint_state['acceleration'] = self.cmd_joint_state['acceleration'] + qddd_des * dt
self.cmd_joint_state['velocity'] = self.cmd_joint_state['velocity'] + self.cmd_joint_state['acceleration'] * dt
self.cmd_joint_state['position'] = self.cmd_joint_state['position'] + self.cmd_joint_state['velocity'] * dt
self.prev_cmd_qdd = self.cmd_joint_state['acceleration']
return self.cmd_joint_state
[docs] def integrate_acc(self, qdd_des, raw_joint_state, dt=None):
dt = self.dt if dt is None else dt
self.filter_joint_state(raw_joint_state)
self.cmd_joint_state['acceleration'] = qdd_des
self.cmd_joint_state['velocity'] = self.cmd_joint_state['velocity'] + qdd_des * dt
self.cmd_joint_state['position'] = self.cmd_joint_state['position'] + self.cmd_joint_state['velocity'] * dt
self.prev_cmd_qdd = self.cmd_joint_state['acceleration']
return self.cmd_joint_state
[docs] def integrate_vel(self, qd_des, raw_joint_state, dt=None):
dt = self.dt if dt is None else dt
self.filter_joint_state(raw_joint_state)
self.cmd_joint_state['velocity'] = qd_des #self.cmd_joint_state['velocity'] + qdd_des * dt
self.cmd_joint_state['position'] = self.cmd_joint_state['position'] + self.cmd_joint_state['velocity'] * dt
return self.cmd_joint_state
[docs] def integrate_pos(self, q_des, raw_joint_state, dt=None):
dt = self.dt if dt is None else dt
self.filter_joint_state(raw_joint_state)
self.cmd_joint_state['velocity'] = (q_des - self.cmd_joint_state['position']) / dt
self.cmd_joint_state['position'] = self.cmd_joint_state['position'] + self.cmd_joint_state['velocity'] * dt
# This needs to also update the acceleration via finite differencing.
raise NotImplementedError
return self.cmd_joint_state