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.
"""
# 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
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(limb=args.limb)
# register shutdown callback
rospy.on_shutdown(js.clean_shutdown)
js.attach_springs()
评论列表
文章目录
