def publish_JointCommand(self):
# initialize JointCommands message
command = JointCommands()
command.name = list(atlasJointNames)
n = len(command.name)
command.position = zeros(n)
command.velocity = zeros(n)
command.effort = zeros(n)
command.kp_position = zeros(n)
command.ki_position = zeros(n)
command.kd_position = zeros(n)
command.kp_velocity = zeros(n)
command.i_effort_min = zeros(n)
command.i_effort_max = zeros(n)
# now get gains from parameter server
for i in xrange(len(command.name)):
name = command.name[i]
command.position[i] = 0.0
command.kp_position[i] = rospy.get_param("atlas_controller/gains/" + name[7::] + "/p")
command.ki_position[i] = rospy.get_param("atlas_controller/gains/" + name[7::] + "/i")
command.kd_position[i] = rospy.get_param("atlas_controller/gains/" + name[7::] + "/d")
command.i_effort_max[i] = rospy.get_param("atlas_controller/gains/" + name[7::] + "/i_clamp")
command.i_effort_min[i] = -command.i_effort_max[i]
for (name, joint) in self.free_joints.items():
counter = 0
appended_name = "atlas::" + name
for item in atlasJointNames:
if item.find(appended_name) != -1:
command.position[counter] = joint["value"]
counter = counter + 1
self.pub.publish(command)
def jointTrajectoryCommandCallback(msg):
while rospy.get_rostime().to_sec() == 0.0:
time.sleep(0.1)
with lock:
if currentJointState == None:
print "/atlas/joint_states is not published"
return
atlasJointNames = [
'atlas::back_lbz', 'atlas::back_mby', 'atlas::back_ubx',
'atlas::l_arm_elx', 'atlas::l_arm_ely', 'atlas::l_arm_mwx', 'atlas::l_arm_shx', 'atlas::l_arm_usy', 'atlas::l_arm_uwy',
'atlas::r_arm_elx', 'atlas::r_arm_ely', 'atlas::r_arm_mwx', 'atlas::r_arm_shx', 'atlas::r_arm_usy', 'atlas::r_arm_uwy',
'atlas::l_leg_kny', 'atlas::l_leg_lax', 'atlas::l_leg_lhy', 'atlas::l_leg_mhx', 'atlas::l_leg_uay', 'atlas::l_leg_uhz',
'atlas::r_leg_kny', 'atlas::r_leg_lax', 'atlas::r_leg_lhy', 'atlas::r_leg_mhx', 'atlas::r_leg_uay', 'atlas::r_leg_uhz',
'atlas::neck_ay']
# initialize JointCommands message
command = JointCommands()
command.name = list(atlasJointNames)
n = len(command.name)
command.position = zeros(n)
command.velocity = zeros(n)
command.effort = zeros(n)
command.kp_position = zeros(n)
command.ki_position = zeros(n)
command.kd_position = zeros(n)
command.kp_velocity = zeros(n)
command.i_effort_min = zeros(n)
command.i_effort_max = zeros(n)
# now get gains from parameter server
for i in xrange(len(command.name)):
name = command.name[i]
command.kp_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/p')
command.ki_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/i')
command.kd_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/d')
command.i_effort_max[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/i_clamp')
command.i_effort_min[i] = -command.i_effort_max[i]
# set up the publisher
pub = rospy.Publisher('/atlas/joint_commands', JointCommands, queue_size=1)
# for each trajectory
for i in xrange(0, len(atlasJointNames)):
# get initial joint positions
initialPosition = array(currentJointState.position)
# first value is time duration
dt = 10.0
# subsequent values are desired joint positions
commandPosition = array([ float(x) for x in currentJointState.position ])
commandPosition[atlasJointNames.index('atlas::back_lbz')] = msg.data # overwrite command angle by subscribed value
# desired publish interval
dtPublish = 0.02
n = ceil(dt / dtPublish)
for ratio in linspace(0, 1, n):
interpCommand = (1-ratio)*initialPosition + ratio * commandPosition
command.position = [ float(x) for x in interpCommand ]
pub.publish(command)
def publish_JointCommand(self):
# initialize JointCommands message
command = JointCommands()
command.name = list(atlasJointNames)
n = len(command.name)
command.position = zeros(n)
command.velocity = zeros(n)
command.effort = zeros(n)
command.kp_position = zeros(n)
command.ki_position = zeros(n)
command.kd_position = zeros(n)
command.kp_velocity = zeros(n)
command.i_effort_min = zeros(n)
command.i_effort_max = zeros(n)
# now get gains from parameter server
for i in xrange(len(command.name)):
name = command.name[i]
command.position[i] = 0.0
command.kp_position[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/p')
command.ki_position[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/i')
command.kd_position[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/d')
command.i_effort_max[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/i_clamp')
command.i_effort_min[i] = -command.i_effort_max[i]
for (name, joint) in self.free_joints.items():
counter = 0
appended_name = "atlas::" + name
for item in atlasJointNames:
if item.find(appended_name) != -1:
command.position[counter] = joint['value']
counter = counter + 1
self.pub.publish(command)
def _send_walking_command_to_atlas(self):
print "entering AtlasController._send_walking_command_to_atlas()"
self._sending_thread_started = True
atlasJointNames = []
n = len(self._current_state.position)
for i in xrange(n):
atlasJointNames.append(
self._joints_info_provider.get_full_name_for_joint(i))
command = JointCommands()
command.name = list(atlasJointNames)
command.position = self._output
command.velocity = self._current_state.velocity
command.effort = self._current_state.effort
command.kp_position = np.zeros(n)
command.ki_position = np.zeros(n)
command.kd_position = np.zeros(n)
command.kp_velocity = np.zeros(n)
command.i_effort_min = np.zeros(n)
command.i_effort_max = np.zeros(n)
# self._get_gains_from_actionlib_server()
gains = self._gains
for i in xrange(n):
command.kp_position[i] = gains[
self._joints_info_provider.get_short_name_for_joint(i)]['p']
command.ki_position[i] = gains[
self._joints_info_provider.get_short_name_for_joint(i)]['i']
command.kd_position[i] = gains[
self._joints_info_provider.get_short_name_for_joint(i)]['d']
command.i_effort_max[i] = gains[
self._joints_info_provider.get_short_name_for_joint(i)][
'i_clamp']
command.i_effort_min[i] = -command.i_effort_max[i]
self._joint_commands_publisher.publish(command)
print "leaving AtlasController._send_walking_command_to_atlas()"
def _send_walking_command_to_atlas(self):
print "entering AtlasController._send_walking_command_to_atlas()"
self._sending_thread_started = True
atlasJointNames = []
n = len(self._current_state.position)
for i in xrange(n):
atlasJointNames.append(
self._joints_info_provider.get_full_name_for_joint(i))
command = JointCommands()
command.name = list(atlasJointNames)
command.position = self._output
command.velocity = self._current_state.velocity
command.effort = self._current_state.effort
command.kp_position = np.zeros(n)
command.ki_position = np.zeros(n)
command.kd_position = np.zeros(n)
command.kp_velocity = np.zeros(n)
command.i_effort_min = np.zeros(n)
command.i_effort_max = np.zeros(n)
# self._get_gains_from_actionlib_server()
gains = self._gains
for i in xrange(n):
command.kp_position[i] = gains[
self._joints_info_provider.get_short_name_for_joint(i)]['p']
command.ki_position[i] = gains[
self._joints_info_provider.get_short_name_for_joint(i)]['i']
command.kd_position[i] = gains[
self._joints_info_provider.get_short_name_for_joint(i)]['d']
command.i_effort_max[i] = gains[
self._joints_info_provider.get_short_name_for_joint(
i)]['i_clamp']
command.i_effort_min[i] = -command.i_effort_max[i]
self._joint_commands_publisher.publish(command)
print "leaving AtlasController._send_walking_command_to_atlas()"
traj_name = sys.argv[2]
if not traj_name in traj_yaml:
print "unable to find trajectory %s in %s" % (traj_name, sys.argv[1])
sys.exit(1)
traj_len = len(traj_yaml[traj_name])
# Setup subscriber to atlas states
rospy.Subscriber("/atlas/joint_states", JointState, jointStatesCallback)
# initialize JointCommands message
command = JointCommands()
command.name = list(atlasJointNames)
n = len(command.name)
command.position = zeros(n)
command.velocity = zeros(n)
command.effort = zeros(n)
command.kp_position = zeros(n)
command.ki_position = zeros(n)
command.kd_position = zeros(n)
command.kp_velocity = zeros(n)
command.i_effort_min = zeros(n)
command.i_effort_max = zeros(n)
# now get gains from parameter server
rospy.init_node('gazebo_drive_simulator')
for i in xrange(len(command.name)):
name = command.name[i]
command.kp_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/p')
command.ki_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/i')
command.kd_position[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/d')
command.i_effort_max[i] = rospy.get_param('atlas_controller/gains/' + name[7::] + '/i_clamp')
traj_name = sys.argv[2]
if not traj_name in traj_yaml:
print "unable to find trajectory %s in %s" % (traj_name, sys.argv[1])
sys.exit(1)
traj_len = len(traj_yaml[traj_name])
# Setup subscriber to atlas states
rospy.Subscriber("/atlas/joint_states", JointState, jointStatesCallback)
# initialize JointCommands message
command = JointCommands()
command.name = list(atlasJointNames)
n = len(command.name)
command.position = zeros(n)
command.velocity = zeros(n)
command.effort = zeros(n)
command.kp_position = zeros(n)
command.ki_position = zeros(n)
command.kd_position = zeros(n)
command.kp_velocity = zeros(n)
command.i_effort_min = zeros(n)
command.i_effort_max = zeros(n)
# now get gains from parameter server
rospy.init_node("gazebo_drive_simulator")
for i in xrange(len(command.name)):
name = command.name[i]
command.kp_position[i] = rospy.get_param("atlas_controller/gains/" + name[7::] + "/p")
command.ki_position[i] = rospy.get_param("atlas_controller/gains/" + name[7::] + "/i")
command.kd_position[i] = rospy.get_param("atlas_controller/gains/" + name[7::] + "/d")
command.i_effort_max[i] = rospy.get_param("atlas_controller/gains/" + name[7::] + "/i_clamp")
def jointTrajectoryCommandCallback(msg):
while rospy.get_rostime().to_sec() == 0.0:
time.sleep(0.1)
with lock:
if currentJointState == None:
print "/atlas/joint_states is not published"
return
atlasJointNames = [
'atlas::back_lbz', 'atlas::back_mby', 'atlas::back_ubx',
'atlas::l_arm_elx', 'atlas::l_arm_ely', 'atlas::l_arm_mwx',
'atlas::l_arm_shx', 'atlas::l_arm_usy', 'atlas::l_arm_uwy',
'atlas::r_arm_elx', 'atlas::r_arm_ely', 'atlas::r_arm_mwx',
'atlas::r_arm_shx', 'atlas::r_arm_usy', 'atlas::r_arm_uwy',
'atlas::l_leg_kny', 'atlas::l_leg_lax', 'atlas::l_leg_lhy',
'atlas::l_leg_mhx', 'atlas::l_leg_uay', 'atlas::l_leg_uhz',
'atlas::r_leg_kny', 'atlas::r_leg_lax', 'atlas::r_leg_lhy',
'atlas::r_leg_mhx', 'atlas::r_leg_uay', 'atlas::r_leg_uhz',
'atlas::neck_ay'
]
# initialize JointCommands message
command = JointCommands()
command.name = list(atlasJointNames)
n = len(command.name)
command.position = zeros(n)
command.velocity = zeros(n)
command.effort = zeros(n)
command.kp_position = zeros(n)
command.ki_position = zeros(n)
command.kd_position = zeros(n)
command.kp_velocity = zeros(n)
command.i_effort_min = zeros(n)
command.i_effort_max = zeros(n)
# now get gains from parameter server
for i in xrange(len(command.name)):
name = command.name[i]
command.kp_position[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/p')
command.ki_position[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/i')
command.kd_position[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/d')
command.i_effort_max[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/i_clamp')
command.i_effort_min[i] = -command.i_effort_max[i]
# set up the publisher
pub = rospy.Publisher('/atlas/joint_commands',
JointCommands,
queue_size=1)
# for each trajectory
for i in xrange(0, len(atlasJointNames)):
# get initial joint positions
initialPosition = array(currentJointState.position)
# first value is time duration
dt = 10.0
# subsequent values are desired joint positions
commandPosition = array(
[float(x) for x in currentJointState.position])
commandPosition[atlasJointNames.index(
'atlas::back_lbz'
)] = msg.data # overwrite command angle by subscribed value
# desired publish interval
dtPublish = 0.02
n = ceil(dt / dtPublish)
for ratio in linspace(0, 1, n):
interpCommand = (
1 - ratio) * initialPosition + ratio * commandPosition
command.position = [float(x) for x in interpCommand]
pub.publish(command)
def start(self):
"""
Sends the command
"""
# Some useful vectors
vector0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
vector1 = [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
vector5 = [5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5]
vector20 = [20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20, 20]
vector100 = [
100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100
]
vector500 = [
500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500
]
# left hand:
# index 0: pointer finger, 1 moves towards middle finger
# index 1: pointer finger, 1 moves towards palm
# index 2: pointer finger, 1 moves towards palm
# index 3: middle finger, 1 moves towards pinky
# index 4: middle finger, 1 moves towards palm
# index 5: middle finger, 1 moves towards palm
# index 6: pinky finger, 1 moves towards away from middle finger
# index 7: pinky finger, 1 moves towards palm
# index 8: pinky finger, 1 moves towards palm
# index 9: thumb finger, 1 moves towards fingers
# index 10: thumb finger, 1 moves towards fingers
# index 11: thumb finger, 1 moves towards towards fingers
leftCommand = JointCommands()
leftCommand.name = [
'left_f0_j0', 'left_f0_j1', 'left_f0_j2', 'left_f1_j0',
'left_f1_j1', 'left_f1_j2', 'left_f2_j0', 'left_f2_j1',
'left_f2_j2', 'left_f3_j0', 'left_f3_j1', 'left_f3_j2'
]
leftCommand.position = vector0
leftCommand.velocity = vector0
leftCommand.effort = vector0
leftCommand.kp_position = vector500 # position error gain
leftCommand.ki_position = vector1 # sum of position error gain
leftCommand.kd_position = vector0 # change in position error over time gain
leftCommand.kp_velocity = vector0 # velocity error gain
leftCommand.i_effort_min = [
-1000, -1000, -1000, -1000, -1000, -1000, -1000, -1000, -1000,
-1000, -1000
]
leftCommand.i_effort_max = [
1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000
]
rightCommand = JointCommands()
rightCommand.name = [
'right_f0_j0', 'right_f0_j1', 'right_f0_j2', 'right_f1_j0',
'right_f1_j1', 'right_f1_j2', 'right_f2_j0', 'right_f2_j1',
'right_f2_j2', 'right_f3_j0', 'right_f3_j1', 'right_f3_j2'
]
rightCommand.position = vector0
rightCommand.velocity = vector0
rightCommand.effort = vector0
rightCommand.kp_position = vector500 # position error gain
rightCommand.ki_position = vector1 # sum of position error gain
rightCommand.kd_position = vector0 # change in position error over time gain
rightCommand.kp_velocity = vector0 # velocity error gain
rightCommand.i_effort_min = [
-1000, -1000, -1000, -1000, -1000, -1000, -1000, -1000, -1000,
-1000, -1000
]
rightCommand.i_effort_max = [
1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000
]
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("Sending command, attempt {0} of {1}.".format(
count + 1, 5))
rightCommand.header.stamp = rospy.Time.now()
self.rightPublisher.publish(rightCommand)
leftCommand.header.stamp = rospy.Time.now()
self.leftPublisher.publish(leftCommand)
count = count + 1
if count < 5:
rospy.sleep(0.5)
rospy.loginfo("Done sending hand commands.")
