def __init__(self, klampt_robot_model):
self.robot = klampt_robot_model
self.state = JointState()
n = self.robot.numLinks()
self.state.name = [self.robot.link(i).getName() for i in range(n)]
self.state.position = []
self.state.velocity = []
self.state.effort = []
# fast indexing structure for partial commands
self.nameToIndex = dict(zip(self.state.name, range(i)))
# Setup publisher of robot states
robot_name = self.robot.getName()
self.pub = rospy.Publisher("/%s/joint_states" % (robot_name, ),
JointState)
# set up the command subscriber
self.firstJointCommand = False
self.newJointCommand = False
self.currentJointCommand = JointCommands()
self.currentJointCommand.name = self.state.name
rospy.Subscriber('/%s/joint_commands' % (robot_name), JointCommands,
self.jointCommandCallback)
return
def __init__(self, hand):
if hand == 'left':
jnt = [
'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'
]
hnd = 'l'
else:
if hand == 'right':
jnt = [
'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'
]
hnd = 'r'
else:
raise ValueError
self._hand = hand
JointCommands_msg_handler.__init__(self,
'sandia_hands/' + hnd + '_hand',
jnt)
self._compub.unregister()
self._compub = rospy.Publisher(
'/' + self.RobotName + '/joint_commands', JointCommands)
self._command = JointCommands()
def __init__(self, finger_idx, j0, j1, j2):
rospy.init_node('finger_test')
# todo: make finger joint commands service; this is a total hack
self.jc_pub = rospy.Publisher("finger_%d/joint_commands" % finger_idx,
JointCommands)
self.jc = JointCommands()
self.jc.name = ["j0", "j1", "j2"]
self.jc.position = [j0, j1, j2]
def __init__(self, argv):
# If specified, we'll only do anything when the scene number matches
self.scene = None
self.arms = []
self.scene = None
# Prepare a message structure that we'll reuse
self.atlasJointNames = [
'atlas::back_lbz', 'atlas::back_mby', 'atlas::back_ubx',
'atlas::neck_ay', 'atlas::l_leg_uhz', 'atlas::l_leg_mhx',
'atlas::l_leg_lhy', 'atlas::l_leg_kny', 'atlas::l_leg_uay',
'atlas::l_leg_lax', 'atlas::r_leg_uhz', 'atlas::r_leg_mhx',
'atlas::r_leg_lhy', 'atlas::r_leg_kny', 'atlas::r_leg_uay',
'atlas::r_leg_lax', 'atlas::l_arm_usy', 'atlas::l_arm_shx',
'atlas::l_arm_ely', 'atlas::l_arm_elx', 'atlas::l_arm_uwy',
'atlas::l_arm_mwx', 'atlas::r_arm_usy', 'atlas::r_arm_shx',
'atlas::r_arm_ely', 'atlas::r_arm_elx', 'atlas::r_arm_uwy',
'atlas::r_arm_mwx'
]
self.joint_command = JointCommands()
self.joint_command.name = list(self.atlasJointNames)
n = len(self.joint_command.name)
self.joint_command.position = [0.0] * n
self.joint_command.velocity = [0.0] * n
self.joint_command.effort = [0.0] * n
self.joint_command.kp_position = [0.0] * n
self.joint_command.ki_position = [0.0] * n
self.joint_command.kd_position = [0.0] * n
self.joint_command.kp_velocity = [0.0] * n
self.joint_command.i_effort_min = [0.0] * n
self.joint_command.i_effort_max = [0.0] * n
if len(argv) > 2:
self.usage()
if len(argv) == 2:
self.scene = int(argv[1])
rospy.init_node('eigen_arm', anonymous=True)
# now get gains from parameter server
for i in xrange(len(self.joint_command.name)):
name = self.joint_command.name[i]
self.joint_command.kp_position[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/p')
self.joint_command.ki_position[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/i')
self.joint_command.kd_position[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/d')
self.joint_command.i_effort_max[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/i_clamp')
self.joint_command.i_effort_min[
i] = -self.joint_command.i_effort_max[i]
self.pub = rospy.Publisher('atlas/joint_commands', JointCommands)
self.joy_sub = rospy.Subscriber('joy', Joy, self.joy_cb)
def __init__(self):
rospy.init_node('finger_cycle_test')
print "waiting for services..."
rospy.wait_for_service('set_joint_limit_policy')
print "releasing joint limits..."
print "done. starting cycle test..."
self.sjlp = rospy.ServiceProxy('set_joint_limit_policy', \
SetJointLimitPolicy)
self.sjlp("none")
self.jc_pub = rospy.Publisher('finger_0/joint_commands', JointCommands)
self.jc = JointCommands()
self.jc.position = [0] * 3
self.jc.name = ["j0", "j1", "j2"]
if len(sys.argv) != 3:
print "usage: traj_yaml.py YAML_FILE TRAJECTORY_NAME"
print " where TRAJECTORY is a dictionary defined in YAML_FILE"
sys.exit(1)
traj_yaml = yaml.load(file(sys.argv[1], 'r'))
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)):
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()"
if len(sys.argv) != 3:
print "usage: traj_yaml.py YAML_FILE TRAJECTORY_NAME"
print " where TRAJECTORY is a dictionary defined in YAML_FILE"
sys.exit(1)
traj_yaml = yaml.load(file(sys.argv[1], "r"))
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)):
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 __init__(self, argv):
# We'll keep the latest joint state here
self.joint_state = None
# Prepare a message structure that we'll reuse
self.atlasJointNames = [
'atlas::back_lbz', 'atlas::back_mby', 'atlas::back_ubx',
'atlas::neck_ay', 'atlas::l_leg_uhz', 'atlas::l_leg_mhx',
'atlas::l_leg_lhy', 'atlas::l_leg_kny', 'atlas::l_leg_uay',
'atlas::l_leg_lax', 'atlas::r_leg_uhz', 'atlas::r_leg_mhx',
'atlas::r_leg_lhy', 'atlas::r_leg_kny', 'atlas::r_leg_uay',
'atlas::r_leg_lax', 'atlas::l_arm_usy', 'atlas::l_arm_shx',
'atlas::l_arm_ely', 'atlas::l_arm_elx', 'atlas::l_arm_uwy',
'atlas::l_arm_mwx', 'atlas::r_arm_usy', 'atlas::r_arm_shx',
'atlas::r_arm_ely', 'atlas::r_arm_elx', 'atlas::r_arm_uwy',
'atlas::r_arm_mwx'
]
if len(argv) < 2 or len(argv) > 4:
self.usage()
# Depending on which arm we're controlling, note the index into the
# joint list; we'll do direct control of 6 joints starting there.
self.num_joints = 6
if argv[1] == 'l':
self.idx_offset1 = self.atlasJointNames.index('atlas::l_arm_usy')
self.idx_offset2 = None
elif argv[1] == 'r':
self.idx_offset1 = self.atlasJointNames.index('atlas::r_arm_usy')
self.idx_offset2 = None
elif argv[1] == 'lr' or argv[1] == 'rl':
self.idx_offset1 = self.atlasJointNames.index('atlas::l_arm_usy')
self.idx_offset2 = self.atlasJointNames.index('atlas::r_arm_usy')
else:
self.usage()
# If specified, we'll only do anything when the scene number matches
self.scene1 = None
self.scene2 = None
if len(argv) >= 3:
self.scene1 = int(argv[2])
if len(argv) == 4:
self.scene2 = int(argv[3])
# This stuff (and much else) belongs in a config file
self.joy_axis_min = 0.0
self.joy_axis_max = 1.0
self.joint_min = -math.pi
self.joint_max = math.pi
self.joint_command = JointCommands()
self.joint_command.name = list(self.atlasJointNames)
n = len(self.joint_command.name)
self.joint_command.position = [0.0] * n
self.joint_command.velocity = [0.0] * n
self.joint_command.effort = [0.0] * n
self.joint_command.kp_position = [0.0] * n
self.joint_command.ki_position = [0.0] * n
self.joint_command.kd_position = [0.0] * n
self.joint_command.kp_velocity = [0.0] * n
self.joint_command.i_effort_min = [0.0] * n
self.joint_command.i_effort_max = [0.0] * n
rospy.init_node('arm_teleop', anonymous=True)
# now get gains from parameter server
for i in xrange(len(self.joint_command.name)):
name = self.joint_command.name[i]
self.joint_command.kp_position[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/p')
self.joint_command.ki_position[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/i')
self.joint_command.kd_position[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/d')
self.joint_command.i_effort_max[i] = rospy.get_param(
'atlas_controller/gains/' + name[7::] + '/i_clamp')
self.joint_command.i_effort_min[
i] = -self.joint_command.i_effort_max[i]
self.joy_sub = rospy.Subscriber('joy', Joy, self.joy_cb)
self.js_sub = rospy.Subscriber('atlas/joint_states', JointState,
self.js_cb)
self.pub = rospy.Publisher('atlas/joint_commands', JointCommands)
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.")
#
# 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.
import roslib; roslib.load_manifest('sandia_hand_teleop')
import rospy
import sys
from sandia_hand_msgs.srv import SimpleGraspSrv, SimpleGraspSrvResponse, SimpleGraspWithSlew, SimpleGraspWithSlewResponse
from sandia_hand_msgs.msg import SimpleGrasp
from osrf_msgs.msg import JointCommands
g_jc_pub = None
g_jc = JointCommands()
g_prev_jc_target = JointCommands()
def grasp_srv(req):
grasp_cb(req.grasp)
return SimpleGraspSrvResponse()
def grasp_slew_srv(req):
#print "going to %s in %.3f" % (req.grasp.name, req.slew_duration)
rate = rospy.Rate(100.0)
t_start = rospy.Time.now()
t_end = t_start + rospy.Duration(req.slew_duration)
while rospy.Time.now() < t_end:
dt = (rospy.Time.now() - t_start).to_sec()
dt_norm = dt / req.slew_duration
#print "%.3f" % dt_norm
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 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)
#!/usr/bin/env python
import roslib; roslib.load_manifest('atlas_teleop')
import rospy, sys, yaml
from osrf_msgs.msg import JointCommands
from atlas_msgs.msg import AtlasCommand
from sensor_msgs.msg import Joy
g_ac_pub = None
g_jc_lh_pub = None
g_jc_rh_pub = None
g_jc = AtlasCommand()
g_jc_rh = JointCommands()
g_jc_lh = JointCommands()
g_vec = [ ]
g_knobs = "unknown"
g_vec_lh = [ ]
g_vec_rh = [ ]
# need to figure out a reasonable way to blend grasps composed of different
# origins... for now, it will ignore the 'origin' data and just use zero...
g_grasps = { 'cyl': { 'origin':[0] * 12,
'grasp': [ 0, 1.5, 1.7, 0, 1.5, 1.7, 0, 1.5, 1.7, -0.2, 0.8, 1.7]},
'sph': { 'origin':[ 0.0, 0, 0, 0.1, 0, 0, 0.0, 0, 0, 0, 0, 0],
'grasp': [ -1.0, 1.4, 1.4, 0.0, 1.4, 1.4, 1.0, 1.4, 1.4, 0, 0.7, 0.7]},
'par': { 'origin':[ 0.0, 0.0, 0.0, 0, -1.4, -1.4, 0.0, -1.4, -1.4, 0.5, 0.0, 0.0],
'grasp': [ 0.0, 1.4, 1.2, 0, -1.4, -1.4, 0.0, -1.4, -1.4, 0.5, 0.7, 0.7]} }
def joy_cb(msg):
global g_ac_pub, g_jc
g_jc.position = [0] * 28
g_jc_lh.position = [0] * 12
g_jc_rh.position = [0] * 12
for x in xrange(0, 28): # start at origin
# 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.
import roslib
roslib.load_manifest('sandia_hand_teleop')
import rospy
import sys
from sandia_hand_msgs.srv import SimpleGraspSrv, SimpleGraspSrvResponse, SimpleGraspWithSlew, SimpleGraspWithSlewResponse
from sandia_hand_msgs.msg import SimpleGrasp
from osrf_msgs.msg import JointCommands
g_jc_pub = None
g_jc = JointCommands()
g_prev_jc_target = JointCommands()
def grasp_srv(req):
grasp_cb(req.grasp)
return SimpleGraspSrvResponse()
def grasp_slew_srv(req):
#print "going to %s in %.3f" % (req.grasp.name, req.slew_duration)
rate = rospy.Rate(100.0)
t_start = rospy.Time.now()
t_end = t_start + rospy.Duration(req.slew_duration)
while rospy.Time.now() < t_end:
dt = (rospy.Time.now() - t_start).to_sec()
