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()