#!/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.
"""
Intera SDK Joint Torque Example: joint springs
"""
import argparse
import importlib
import rospy
from dynamic_reconfigure.server import Server
from std_msgs.msg import Empty
import intera_interface
from intera_interface import CHECK_VERSION
class JointSprings(object):
"""
Virtual Joint Springs class for torque example.
@param limb: limb on which to run joint springs example
@param reconfig_server: dynamic reconfigure server
JointSprings class contains methods for the joint torque example allowing
moving the limb to a neutral location, entering torque mode, and attaching
virtual springs.
"""
def __init__(self, reconfig_server, limb = "right"):
self._dyn = reconfig_server
# control parametersd 控制参数
self._rate = 1000.0 # Hz
self._missed_cmds = 20.0 # Missed cycles before triggering timeout
# create our limb instance创建我们的肢体实例
self._limb = intera_interface.Limb(limb)
# initialize parameters
self._springs = dict()
self._damping = dict()
self._start_angles = dict()
# create cuff disable publisher创建袖口禁用发布者
cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
# verify robot is enabled验证机器人已启用
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
def _update_parameters(self):
for joint in self._limb.joint_names():
self._springs[joint] = self._dyn.config[joint[-2:] + '_spring_stiffness']
self._damping[joint] = self._dyn.config[joint[-2:] + '_damping_coefficient']
def _update_forces(self):
"""
Calculates the current angular difference between the start position
and the current joint positions applying the joint torque spring forces
as defined on the dynamic reconfigure server.
计算当前起始位置之间的角度差
以及施加关节扭矩弹簧力的当前关节位置
如动态重新配置服务器上定义的。
"""
# get latest spring constants获取最新的弹簧常数
self._update_parameters()
# disable cuff interaction禁用袖带交互
self._pub_cuff_disable.publish()
# create our command dict创建我们的命令字典
cmd = dict()
# record current angles/velocities记录当前角度/速度
cur_pos = self._limb.joint_angles()
cur_vel = self._limb.joint_velocities()
# calculate current forces计算当前力
for joint in self._start_angles.keys():
# spring portion弹簧部分
cmd[joint] = self._springs[joint] * (self._start_angles[joint] -
cur_pos[joint])
# damping portion阻尼部分
cmd[joint] -= self._damping[joint] * cur_vel[joint]
# command new joint torques命令新的关节扭矩
self._limb.set_joint_torques(cmd)
def move_to_neutral(self):
"""
Moves the limb to neutral location.将肢体移动到中立位置。
"""
self._limb.move_to_neutral()
def attach_springs(self):
"""
Switches to joint torque mode and attached joint springs to current
joint positions.
切换到关节扭矩模式，并将关节弹簧连接到电流
联合阵地。111
"""
# record initial joint angles记录初始关节角度
self._start_angles = self._limb.joint_angles()
# set control rate设置控制速率
control_rate = rospy.Rate(self._rate)
# for safety purposes, set the control rate command timeout.
# if the specified number of command cycles are missed, the robot
# will timeout and return to Position Control Mode#出于安全目的，设置控制速率命令超时。
#如果错过指定数量的命令周期，机器人
#将超时并返回位置控制模式
self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)
# loop at specified rate commanding new joint torques以指定速率循环指令新的接头扭矩
while not rospy.is_shutdown():
if not self._rs.state().enabled:
rospy.logerr("Joint torque example failed to meet "
"specified control rate timeout.")
break
self._update_forces()
control_rate.sleep()
def clean_shutdown(self):
"""
Switches out of joint torque mode to exit cleanly切换出关节扭矩模式，干净利落地退出
"""
print("\nExiting example...")
self._limb.exit_control_mode()
def main():
"""RSDK Joint Torque Example: Joint Springs
Moves the default limb to a neutral location and enters
torque control mode, attaching virtual springs (Hooke's Law)
to each joint maintaining the start position.
Run this example and interact by grabbing, pushing, and rotating
each joint to feel the torques applied that represent the
virtual springs attached. You can adjust the spring
constant and damping coefficient for each joint using
dynamic_reconfigure.RSDK接头扭矩示例:接头弹簧
将默认肢体移动到中性位置并输入
扭矩控制模式，连接虚拟弹簧(胡克定律)
保持起始位置。
运行此示例，通过抓取、推动和旋转进行交互
每个关节感受所施加的扭矩，这些扭矩代表
虚拟弹簧连接。你可以调节弹簧
每个接头的常数和阻尼系数使用
动态重新配置。
"""
# Querying the parameter server to determine Robot model and limb name(s)查询参数服务器以确定机器人模型和肢体名称
rp = intera_interface.RobotParams()
valid_limbs = rp.get_limb_names()
if not valid_limbs:
rp.log_message(("Cannot detect any limb parameters on this robot. "
"Exiting."), "ERROR")
robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize()
# Parsing Input Arguments解析输入参数
arg_fmt = argparse.ArgumentDefaultsHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt)
parser.add_argument(
"-l", "--limb", dest="limb", default=valid_limbs[0],
choices=valid_limbs,
help='limb on which to attach joint springs'
)
args = parser.parse_args(rospy.myargv()[1:])
# Grabbing Robot-specific parameters for Dynamic Reconfigure抓取机器人特定参数进行动态重新配置
config_name = ''.join([robot_name,"JointSpringsExampleConfig"])
config_module = "intera_examples.cfg"
cfg = importlib.import_module('.'.join([config_module,config_name]))
# Starting node connection to ROS开始节点连接到ROS
print("Initializing node... ")
rospy.init_node("sdk_joint_torque_springs_{0}".format(args.limb))
dynamic_cfg_srv = Server(cfg, lambda config, level: config)
js = JointSprings(dynamic_cfg_srv, limb=args.limb)
# register shutdown callback寄存器关闭回调
rospy.on_shutdown(js.clean_shutdown)
js.move_to_neutral()
js.attach_springs()
if __name__ == "__main__":
main()
网友评论