#! /usr/bin/env python
# Copyright (c) 2013-2018, Rethink Robotics Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

from dynamic_reconfigure.parameter_generator_catkin import (
    ParameterGenerator,
    double_t,
)

gen = ParameterGenerator()

joints = ('j0', 'j1', 'j2', 'j3', 'j4', 'j5', 'j6')

msg = (" - Joint spring stiffness (k). Hooke's Law.",
       " - Joint damping coefficient (c).")
min = 0.0
default_spring = (10.0, 15.0, 5.0, 5.0, 3.0, 2.0, 1.5)
default_damping = (0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1)
max_spring = (30.0, 30.0, 15.0, 15.0, 9.0, 4.0, 4.0)
max_damping = (10.0, 7.5, 7.5, 5.0, 1.5, 1.5, 1.0)

for idx, joint in enumerate(joints):
    gen.add(
        joint + "_spring_stiffness", double_t, 0, joint + msg[0],
        default_spring[idx], min, max_spring[idx]
        )
    gen.add(
        joint + "_damping_coefficient", double_t, 0, joint + msg[1],
        default_damping[idx], min, max_damping[idx]
        )

exit(gen.generate('joint_torque', '', 'SawyerJointSpringsExample'))
