<commit_before>#
# subscribe to thw joint angles and raw forces topics,  and provide FK
# etc.
#
# Copyright (c) 2009, Georgia Tech Research Corporation
# All rights reserved.
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the Georgia Tech Research Corporation nor the
#       names of its contributors may be used to endorse or promote products
#       derived from this software without specific prior written permission.
# 
# THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#

# Author: <NAME>

import math
import numpy as np
import copy
import sys

import roslib; roslib.load_manifest('hrl_cody_arms')
import rospy

from cody_arm_kinematics import CodyArmKinematics
from equilibrium_point_control.hrl_arm import HRLArm
import hrl_lib.viz as hv

from hrl_msgs.msg import FloatArray
from std_msgs.msg import Header, Bool, Empty

from visualization_msgs.msg import Marker


# used in client and server.
def get_joint_name_list(arm):
    if arm == 'r':
        return ['m3joint_ma1_j%d'%i for i in range(7)]
    else:
        return ['m3joint_ma2_j%d'%i for i in range(7)]


class CodyArmClient(HRLArm):
    # arm - 'r' or 'l'
    def __init__(self, arm):
        kinematics = CodyArmKinematics(arm)
        HRLArm.__init__(self, kinematics)

        #stiffness from .yaml file [1800., 1300., 350., 600., 60., 80., 60.] mN-meter/deg
        self.nom_kp = [103, 74.5, 20.1, 34.4, 3.44, 4.58, 3.44] #N-m/rad
        if arm == 'r':
            #damping from .yaml file [100., 120., 10., 25., 1.25, 0.3, 0.25] mN-meter-sec/deg
            self.nom_kd = [5.73, 6.88, 0.573, 1.43, 0.0716, 0.0172, 0.0143] #N-m-s/deg
        elif arm == 'l':
            #damping from .yaml file [80., 60., 10., 15., 1.25, 0.3, 0.20] mN-meter-sec/deg
            self.nom_kd = [4.58, 3.44, 0.573, 0.859, 0.0716, 0.0172, 0.0115] #N-m-s/deg<commit_msg>This is so that we are only using the first 5 values of the stiffness and alpha parameters.  Will need to update if use a different meka robot<commit_after>#
# subscribe to thw joint angles and raw forces topics,  and provide FK
# etc.
#
# Copyright (c) 2009, Georgia Tech Research Corporation
# All rights reserved.
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the Georgia Tech Research Corporation nor the
#       names of its contributors may be used to endorse or promote products
#       derived from this software without specific prior written permission.
# 
# THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#

# Author: <NAME>

import math
import numpy as np
import copy
import sys

import roslib; roslib.load_manifest('hrl_cody_arms')
import rospy

from cody_arm_kinematics import CodyArmKinematics
from equilibrium_point_control.hrl_arm import HRLArm
import hrl_lib.viz as hv

from hrl_msgs.msg import FloatArray
from std_msgs.msg import Header, Bool, Empty

from visualization_msgs.msg import Marker


# used in client and server.
def get_joint_name_list(arm):
    if arm == 'r':
        return ['m3joint_ma1_j%d'%i for i in range(7)]
    else:
        return ['m3joint_ma2_j%d'%i for i in range(7)]


class CodyArmClient(HRLArm):
    # arm - 'r' or 'l'
    def __init__(self, arm):
        kinematics = CodyArmKinematics(arm)
        HRLArm.__init__(self, kinematics)

        #stiffness from .yaml file [1800., 1300., 350., 600., 60., 80., 60.] mN-meter/deg
        self.nom_kp = [103, 74.5, 20.1, 34.4, 3.44] #N-m/rad
        # we don't use last two values becuase they are position controlled  - 4.58, 3.44] 

        if arm == 'r':
            #damping from .yaml file [100., 120., 10., 25., 1.25, 0.3, 0.25] mN-meter-sec/deg
            self.nom_kd = [5.73, 6.88, 0.573, 1.43, 0.0716] #N-m-s/deg
            # we don't use last two values becuase they are position controlled  - 0.0172, 0.0143]
        elif arm == 'l':
            #damping from .yaml file [80., 60., 10., 15., 1.25, 0.3, 0.20] mN-meter-sec/deg
            self.nom_kd = [4.58, 3.44, 0.573, 0.859, 0.0716] #N-m-s/deg
            # we don't use last two values becuase they are position controlled  - 0.0172, 0.0115
{'fraction_non_alphanumeric': 0.08609683948310758, 'fraction_numerical': 0.044371788883699206, 'mean_word_length': 3.325925925925926, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6889344', 'n_tokens_mistral': 2262, 'n_tokens_neox': 1918, 'n_words': 825}