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()"
# 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') 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
# 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") 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
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.")