def __init__(self, limb = "right"):
# control parameters
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._des_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")
rospy.Subscriber("desired_joint_pos", JointState, self._set_des_pos)
rospy.Subscriber("release_spring", Float32, self._release)
rospy.Subscriber("imp_ctrl_active", Int64, self._imp_ctrl_active)
self.max_stiffness = 20
self.time_to_maxstiffness = .3 ######### 0.68
self.t_release = rospy.get_time()
self._imp_ctrl_is_active = True
for joint in self._limb.joint_names():
self._springs[joint] = 30
self._damping[joint] = 4
评论列表
文章目录