def hebi_set_gains(fullname, Kp, Kv, lam):
# Check that the parameters are non-negative!
if (Kp < 0.0) or (Kv < 0.0) or (lam < 0.0):
rospy.logerr('Kp/Kv/Lambda must be non-negative!')
raise ValueError('Negative gain/frequency values')
# Convert the filter bandwidth into the filter constant, saturate
# to 1.0, and revert back to frequency (in rad/s). Use the Tustin
# approximation (blinear mapping) for best correspondence.
lp = min(1.0, 2.0 * lam / (2000.0 + lam))
lam = 2000.0 * lp / (2.0 - lp)
# Report.
fmt = ("Joint %-20s set to Kp %6.1f Nm/rad, " +
"Kv %6.2f PWM/(rad/s), Lam %6.1f rad/s = %6.1f Hz")
rospy.loginfo(fmt % (fullname, Kp, Kv, lam, lam / (2.0 * math.pi)))
# Populate the command with the things we want to change.
command = CommandMsg()
command.name = [fullname]
command.settings.name = [fullname]
command.settings.save_current_settings = [False]
command.settings.position_gains.name = [fullname]
command.settings.position_gains.kp = [Kp]
command.settings.velocity_gains.name = [fullname]
command.settings.velocity_gains.kp = [Kv]
command.settings.velocity_gains.output_lowpass = [lp]
# Call the /hebiros/robot/send_command_with_acknowledgement service.
service = '/hebiros/robot/send_command_with_acknowledgement'
try:
proxy = rospy.ServiceProxy(service, SendCommandWithAcknowledgementSrv)
resp = proxy(command)
except rospy.ServiceException, e:
rospy.logerr("Failed to set gains!")
rospy.logerr("Service call failed: %s" % e)
raise
if __name__ == '__main__':
parser = parse_args(rospy.myargv(sys.argv[1:]))
rospy.init_node(name="effort_ctrl", anonymous=True)
# Set up HEBI ROS interface
hebi_families = parser.module.split('/')[0]
hebi_names = parser.module.split('/')[1]
hebi_wrap = HebirosWrapper(parser.hebi_group, [hebi_families],
[hebi_names])
hebi_mapping = [parser.module]
cmd_msg = CommandMsg()
cmd_msg.name = hebi_mapping
cmd_msg.settings.name = hebi_mapping
cmd_msg.settings.effort_gains.name = hebi_mapping
cmd_msg.settings.effort_gains.kp = [1]
cmd_msg.settings.effort_gains.ki = [0]
cmd_msg.settings.effort_gains.kd = [0]
cmd_msg.settings.effort_gains.i_clamp = [0]
hebi_wrap.send_command_with_acknowledgement(cmd_msg)
_hold_position = False
_hold_joint_effort = None
def _hold_position_cb(msg):
if not rospy.is_shutdown() and _hold_position:
jointstate = JointState()
def __init__(self, hebi_group_name, hebi_mapping):
rospy.loginfo("Creating SkidSteer instance...")
hebi_families = []
hebi_names = []
for k, v in hebi_mapping.iteritems():
family, name = v.split('/')
hebi_families.append(family)
hebi_names.append(name)
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_mapping = [motor for key, motor in hebi_mapping.iteritems()]
self.hebi_mapping_dict = hebi_mapping
# jt information populated by self._feedback_cb
self._current_jt_pos = {}
self._current_jt_vel = {}
self._current_jt_eff = {}
self._joint_state_pub = None
# Create a service client to create a group
set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names',
AddGroupFromNamesSrv)
# Topic to receive feedback from a group
self.hebi_group_feedback_topic = "/hebiros/" + hebi_group_name + "/feedback/joint_state"
rospy.loginfo(" hebi_group_feedback_topic: %s",
"/hebiros/" + hebi_group_name + "/feedback/joint_state")
# Topic to send commands to a group
self.hebi_group_command_topic = "/hebiros/" + hebi_group_name + "/command/joint_state"
rospy.loginfo(" hebi_group_command_topic: %s",
"/hebiros/" + hebi_group_name + "/command/joint_state")
# Call the /hebiros/add_group_from_names service to create a group
rospy.loginfo(" Waiting for AddGroupFromNamesSrv at %s ...",
'/hebiros/add_group_from_names')
rospy.wait_for_service('/hebiros/add_group_from_names'
) # block until service server starts
rospy.loginfo(" AddGroupFromNamesSrv AVAILABLE.")
set_hebi_group(hebi_group_name, hebi_names, hebi_families)
# Create a service client to set group settings
change_group_settings = rospy.ServiceProxy(
"/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement",
SendCommandWithAcknowledgementSrv)
rospy.loginfo(
" Waiting for SendCommandWithAcknowledgementSrv at %s ...",
"/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement")
rospy.wait_for_service("/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement"
) # block until service server starts
cmd_msg = CommandMsg()
cmd_msg.name = self.hebi_mapping
cmd_msg.settings.name = cmd_msg.name
cmd_msg.settings.velocity_gains.name = cmd_msg.name
cmd_msg.settings.position_gains.kp = [10] * WHEELS # FIXME: Tune
cmd_msg.settings.position_gains.ki = [5] * WHEELS # FIXME: Tune
cmd_msg.settings.position_gains.kd = [0.1] * WHEELS # FIXME: Tune
cmd_msg.settings.position_gains.i_clamp = [0.1] * WHEELS # FIXME: Tune
change_group_settings(cmd_msg)
# Feedback/Command
self.fbk_sub = rospy.Subscriber(self.hebi_group_feedback_topic,
JointState, self._feedback_cb)
self.cmd_pub = rospy.Publisher(self.hebi_group_command_topic,
JointState,
queue_size=1)
# Twist Subscriber
self._cmd_vel_sub = rospy.Subscriber("/skidsteer/cmd_vel/", Twist,
self._cmd_vel_cb)
self.last_vel_cmd = None
self.linear_displacement_limit = 0.075 # m
self.angular_displacement_limit = 0.65 # rad
# Wait for connections to be set up
rospy.loginfo("Wait for ROS connections to be set up...")
while not rospy.is_shutdown() and len(self._current_jt_pos) < len(
self.hebi_mapping):
rospy.sleep(0.1)
self._joint_state_pub = rospy.Publisher('/joint_states',
JointState,
queue_size=1)
self._loop_rate = rospy.Rate(200)
self.lin_vel = 7.5
self.ang_vel = 7.5
rospy.loginfo("Done creating SkidSteer instance...")
def __init__(self, hebi_group_name, hebi_mapping, hebi_gains, urdf_str, base_link, end_link):
rospy.loginfo("Creating {} instance".format(self.__class__.__name__))
self.openmeta_testbench_manifest_path = rospy.get_param('~openmeta/testbench_manifest_path', None)
if self.openmeta_testbench_manifest_path is not None:
self._testbench_manifest = load_json_file(self.openmeta_testbench_manifest_path)
# TestBench Parameters
self._params = {}
for tb_param in self._testbench_manifest['Parameters']:
self._params[tb_param['Name']] = tb_param['Value'] # WARNING: If you use these values - make sure to check the type
# TestBench Metrics
self._metrics = {}
for tb_metric in self._testbench_manifest['Metrics']: # FIXME: Hmm, this is starting to look a lot like OpenMDAO...
self._metrics[tb_metric['Name']] = tb_metric['Value']
if hebi_gains is None:
hebi_gains = {
'p': [float(self._params['p_gain'])]*3,
'i': [float(self._params['i_gain'])]*3,
'd': [float(self._params['d_gain'])]*3
}
hebi_families = []
hebi_names = []
for actuator in hebi_mapping:
family, name = actuator.split('/')
hebi_families.append(family)
hebi_names.append(name)
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_wrap = HebirosWrapper(hebi_group_name, hebi_families, hebi_names)
# Set PID Gains
cmd_msg = CommandMsg()
cmd_msg.name = hebi_mapping
cmd_msg.settings.name = hebi_mapping
cmd_msg.settings.control_strategy = [4, 4, 4]
cmd_msg.settings.position_gains.name = hebi_mapping
cmd_msg.settings.position_gains.kp = hebi_gains['p']
cmd_msg.settings.position_gains.ki = hebi_gains['i']
cmd_msg.settings.position_gains.kd = hebi_gains['d']
cmd_msg.settings.position_gains.i_clamp = [0.25, 0.25, 0.25] # TODO: Tune me.
self.hebi_wrap.send_command_with_acknowledgement(cmd_msg)
if base_link is None or end_link is None:
robot = Robot.from_xml_string(urdf_str)
if not base_link:
base_link = robot.get_root()
# WARNING: There may be multiple leaf nodes
if not end_link:
end_link = [x for x in robot.link_map.keys() if x not in robot.child_map.keys()][0]
# pykdl
self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link, end_link)
self._active_joints = self.kdl_fk.get_joint_names()
# joint data
self.position_fbk = [0.0]*self.hebi_wrap.hebi_count
self.velocity_fbk = [0.0]*self.hebi_wrap.hebi_count
self.effort_fbk = [0.0]*self.hebi_wrap.hebi_count
# joint state publisher
while not rospy.is_shutdown() and len(self.hebi_wrap.get_joint_positions()) < len(self.hebi_wrap.hebi_mapping):
rospy.sleep(0.1)
self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
self.hebi_wrap.add_feedback_callback(self._joint_state_cb)
# Set up Waypoint/Trajectory objects
self.start_wp = WaypointMsg()
self.start_wp.names = self.hebi_wrap.hebi_mapping
self.end_wp = copy.deepcopy(self.start_wp)
self.goal = TrajectoryGoal()
self.goal.waypoints = [self.start_wp, self.end_wp]
self._hold_positions = [0.0]*3
self._hold = True
self.jointstate = JointState()
self.jointstate.name = self.hebi_wrap.hebi_mapping
rospy.sleep(1.0)
def __init__(self, hebi_group_name, hebi_mapping, leg_base_links,
leg_end_links):
rospy.loginfo("Creating Hexapod instance...")
hebi_families = []
hebi_names = []
for leg in hebi_mapping:
for actuator in leg:
family, name = actuator.split('/')
hebi_families.append(family)
hebi_names.append(name)
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_mapping = hebi_mapping
self.hebi_mapping_flat = self._flatten(self.hebi_mapping)
# jt information populated by self._feedback_cb
self._current_jt_pos = {}
self._current_jt_vel = {}
self._current_jt_eff = {}
self._joint_state_pub = None
self._hold_leg_list = [False, False, False, False, False, False]
self._hold_leg_positions = self._get_list_of_lists()
# Create a service client to create a group
set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names',
AddGroupFromNamesSrv)
# Topic to receive feedback from a group
self.hebi_group_feedback_topic = "/hebiros/" + hebi_group_name + "/feedback/joint_state"
rospy.loginfo(" hebi_group_feedback_topic: %s",
"/hebiros/" + hebi_group_name + "/feedback/joint_state")
# Topic to send commands to a group
self.hebi_group_command_topic = "/hebiros/" + hebi_group_name + "/command/joint_state"
rospy.loginfo(" hebi_group_command_topic: %s",
"/hebiros/" + hebi_group_name + "/command/joint_state")
# Call the /hebiros/add_group_from_names service to create a group
rospy.loginfo(" Waiting for AddGroupFromNamesSrv at %s ...",
'/hebiros/add_group_from_names')
rospy.wait_for_service('/hebiros/add_group_from_names'
) # block until service server starts
rospy.loginfo(" AddGroupFromNamesSrv AVAILABLE.")
set_hebi_group(hebi_group_name, hebi_names, hebi_families)
# Create a service client to set group settings
change_group_settings = rospy.ServiceProxy(
"/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement",
SendCommandWithAcknowledgementSrv)
rospy.loginfo(
" Waiting for SendCommandWithAcknowledgementSrv at %s ...",
"/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement")
rospy.wait_for_service("/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement"
) # block until service server starts
cmd_msg = CommandMsg()
cmd_msg.name = self.hebi_mapping_flat
cmd_msg.settings.name = self.hebi_mapping_flat
cmd_msg.settings.position_gains.name = self.hebi_mapping_flat
cmd_msg.settings.position_gains.kp = [5, 8, 2] * LEGS
cmd_msg.settings.position_gains.ki = [0.001] * LEGS * ACTUATORS_PER_LEG
cmd_msg.settings.position_gains.kd = [0] * LEGS * ACTUATORS_PER_LEG
cmd_msg.settings.position_gains.i_clamp = [
0.25
] * LEGS * ACTUATORS_PER_LEG # TODO: Tune this. Setting it low for testing w/o restarting Gazebo
change_group_settings(cmd_msg)
# Feedback/Command
self.fbk_sub = rospy.Subscriber(self.hebi_group_feedback_topic,
JointState, self._feedback_cb)
self.cmd_pub = rospy.Publisher(self.hebi_group_command_topic,
JointState,
queue_size=1)
self._hold_position = False
self._hold_joint_states = {}
# TrajectoryAction client
self.trajectory_action_client = SimpleActionClient(
"/hebiros/" + hebi_group_name + "/trajectory", TrajectoryAction)
rospy.loginfo(" Waiting for TrajectoryActionServer at %s ...",
"/hebiros/" + hebi_group_name + "/trajectory")
self.trajectory_action_client.wait_for_server(
) # block until action server starts
rospy.loginfo(" TrajectoryActionServer AVAILABLE.")
# Twist Subscriber
self._cmd_vel_sub = rospy.Subscriber("/hexapod/cmd_vel/", Twist,
self._cmd_vel_cb)
self.last_vel_cmd = None
self.linear_displacement_limit = 0.075 # m
self.angular_displacement_limit = 0.65 # rad
# Check ROS Parameter server for robot_description URDF
urdf_str = ""
urdf_loaded = False
while not rospy.is_shutdown() and not urdf_loaded:
if rospy.has_param('/robot_description'):
urdf_str = rospy.get_param('/robot_description')
urdf_loaded = True
rospy.loginfo(
"Pulled /robot_description from parameter server.")
else:
rospy.sleep(0.01) # sleep for 10 ms of ROS time
# pykdl_utils setup
self.robot_urdf = URDF.from_xml_string(urdf_str)
self.kdl_fk_base_to_leg_base = [
KDLKinematics(self.robot_urdf, 'base_link', base_link)
for base_link in leg_base_links
]
self.kdl_fk_leg_base_to_eff = [
KDLKinematics(self.robot_urdf, base_link, end_link)
for base_link, end_link in zip(leg_base_links, leg_end_links)
]
# trac-ik setup
self.trac_ik_leg_base_to_end = [
IK(
base_link,
end_link,
urdf_string=urdf_str,
timeout=0.01, # TODO: Tune me
epsilon=1e-4,
solve_type="Distance")
for base_link, end_link in zip(leg_base_links, leg_end_links)
]
self.ik_pos_xyz_bounds = [0.01, 0.01, 0.01]
self.ik_pos_wxyz_bounds = [31416.0, 31416.0, 31416.0
] # NOTE: This implements position-only IK
# Wait for connections to be set up
rospy.loginfo("Wait for ROS connections to be set up...")
while not rospy.is_shutdown() and len(self._current_jt_pos) < len(
self.hebi_mapping_flat):
rospy.sleep(0.1)
# Set up joint state publisher
self._joint_state_pub = rospy.Publisher('/joint_states',
JointState,
queue_size=1)
self._loop_rate = rospy.Rate(20)
# leg joint home pos Hip, Knee, Ankle
self.leg_jt_home_pos = [
[0.0, +0.26, -1.57], # Leg 1
[0.0, -0.26, +1.57], # Leg 2
[0.0, +0.26, -1.57], # Leg 3
[0.0, -0.26, +1.57], # Leg 4
[0.0, +0.26, -1.57], # Leg 5
[0.0, -0.26, +1.57]
] # Leg 6
# leg end-effector home position
self.leg_eff_home_pos = self._get_leg_base_to_eff_fk(
self.leg_jt_home_pos)
# leg step height relative to leg base link
self.leg_eff_step_height = [[]] * LEGS # relative to leg base
for i, fk_solver in enumerate(self.kdl_fk_base_to_leg_base):
base_to_leg_base_rot = fk_solver.forward([])[:3, :3]
step_ht_chassis = np.array([0, 0, STEP_HEIGHT])
step_ht_leg_base = np.dot(base_to_leg_base_rot, step_ht_chassis)
self.leg_eff_step_height[i] = step_ht_leg_base.tolist()[0]
self._odd_starts = True
rospy.loginfo("Done creating Hexapod instance...")
def main():
# ROS stuff
rospy.init_node('hexapod_leg_fsm_node', anonymous=True)
# Parse node args
parser = parse_args(sys.argv[1:])
hebi_group_name = parser.hebi_group_name
hebi_mapping_hip = parser.hebi_mapping_hip
hebi_mapping_knee = parser.hebi_mapping_knee
hebi_mapping_ankle = parser.hebi_mapping_ankle
base_link_name = parser.base_link_name
end_link_name = parser.end_link_name
from_master_topic = parser.from_master_topic
to_master_topic = parser.to_master_topic
step_height = parser.step_height
# Hardware interface
hebi_mapping = [hebi_mapping_hip, hebi_mapping_knee, hebi_mapping_ankle]
hebi_families = []
hebi_names = []
for actuator in hebi_mapping:
family, name = actuator.split('/')
hebi_families.append(family)
hebi_names.append(name)
hebi_wrap = HebirosWrapper(hebi_group_name, hebi_families, hebi_names)
# Set PID Gains
cmd_msg = CommandMsg()
cmd_msg.name = hebi_mapping
cmd_msg.settings.name = hebi_mapping
cmd_msg.settings.position_gains.name = hebi_mapping
cmd_msg.settings.position_gains.kp = [5, 8, 2]
cmd_msg.settings.position_gains.ki = [0.001, 0.001, 0.001]
cmd_msg.settings.position_gains.kd = [0, 0, 0]
cmd_msg.settings.position_gains.i_clamp = [0.25, 0.25, 0.25] # TODO: Tune me.
hebi_wrap.send_command_with_acknowledgement(cmd_msg)
# Get URDF
urdf_str = ""
urdf_loaded = False
while not rospy.is_shutdown() and not urdf_loaded:
if rospy.has_param('robot_description'):
urdf_str = rospy.get_param('robot_description')
urdf_loaded = True
rospy.loginfo("Pulled {} from parameter server.".format(rospy.resolve_name('robot_description')))
else:
rospy.sleep(0.01) # sleep for 10 ms of ROS time
### CREATE LEG STATE INSTANCES ###
wait_for_master = WaitForMaster(hebiros_wrapper=hebi_wrap, from_master_topic=from_master_topic, to_master_topic=to_master_topic)
step = Step(hebiros_wrapper=hebi_wrap, urdf_str=urdf_str, base_link=base_link_name, end_link=end_link_name, step_height=step_height)
push = Push(hebiros_wrapper=hebi_wrap, urdf_str=urdf_str, base_link=base_link_name, end_link=end_link_name)
### CREATE TOP SM ###
top = StateMachine(outcomes=['exit','success'])
### INITIALIZE USERDATA ###
top.userdata.prev_joint_pos = [0,0,0]
top.userdata.target_end_link_point = None
top.userdata.execution_time = None
top.userdata.active_joints = None
remapping_list = ['prev_joint_pos', 'prev_joint_pos', 'target_end_link_point', 'execution_time', 'active_joints']
remapping_dict = {userdata: userdata for userdata in remapping_list}
with top:
StateMachine.add('WAIT_FOR_MASTER', wait_for_master,
transitions={'exit':'exit', 'step':'STEP', 'push':'PUSH'},
remapping=remapping_dict)
StateMachine.add('STEP', step,
transitions={'ik_failed':'WAIT_FOR_MASTER', 'success':'WAIT_FOR_MASTER'},
remapping=remapping_dict)
StateMachine.add('PUSH', push,
transitions={'ik_failed':'WAIT_FOR_MASTER', 'success':'WAIT_FOR_MASTER'},
remapping=remapping_dict)
sis = smach_ros.IntrospectionServer(str(rospy.get_name()), top, '/SM_ROOT' + str(rospy.get_name()))
sis.start()
#user_input = raw_input("Please press the 'Return/Enter' key to start executing \
# - pkg: hexapod_leg_fsm_node.py | node: " + str(rospy.get_name()) + "\n")
print("Input received. Executing...")
top.execute()
rospy.spin()
sis.stop()
print("\nExiting " + str(rospy.get_name()))