コード例 #1
0
 def _proceed_with_walking(self):
     message = AtlasCommand()
     message.position = self._current_position
     # position_list = list(self._current_position)
     # if self._global_record_counter == len(self._position_record)-1:
     #     self._global_record_counter = 0
     # else:
     #     self._global_record_counter += 1
     # for i in xrange(5,16):
     #     position_list[i] = self._position_record[
     #         self._global_record_counter][i]
     # message.position = tuple(position_list)
     message.k_effort = [255] * 28
     self._atlas_command_publisher.publish(message)
     time.sleep(0.02)
コード例 #2
0
ファイル: kdl_kinect.py プロジェクト: Aharobot/drc
def initAtlasCommand():
    n = 28
    command = AtlasCommand()
    command.position    = [0] * n
    command.velocity    = [0] * n
    command.effort      = [0] * n
    command.kp_position = [0] * n
    command.ki_position = [0] * n
    command.kd_position = [0] * n
    command.kp_velocity = [0] * n
    command.k_effort    = [0] * n
    return command
コード例 #3
0
    def __init__(self):
        # initialize atlas joint names
        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'
        ]
        # initialize new constants
        self._kp_position = [
            800, 4000, 2000, 20, 1000, 1200, 1100, 1000, 1200, 600, 1000, 1200,
            1100, 1000, 1200, 600, 2000, 1000, 30, 700, 50, 100, 2000, 1000,
            30, 700, 50, 100
        ]
        self._kd_position = [
            0, 2, 1, 1, 0, 0, 0, 10, 0, 0, 0, 0, 0, 10, 0, 0, 3, 10, 2, 3, 0.1,
            0.2, 3, 10, 2, 3, 0.1, 0.2
        ]
        # joint states subscriber
        self._currentJointState = JointState()

        # initialize command
        self._command = AtlasCommand()
        #self._command.name = list(self._atlasJointNames)
        n = len(self._atlasJointNames)
        self._command.velocity = zeros(n)
        self._command.effort = zeros(n)
        self._command.kp_position = zeros(n)
        self._command.ki_position = zeros(n)
        self._command.kd_position = zeros(n)
        self._command.kp_velocity = zeros(n)
        self._command.i_effort_min = zeros(n)
        self._command.i_effort_max = zeros(n)
        # Set k_effort to 255 to indicate that we want PID control of each joint
        self._command.k_effort = [255] * n
        for i in xrange(len(self._atlasJointNames)):
            name = self._atlasJointNames[i]
            self._command.kp_position[i] = self._kp_position[i]
            self._command.ki_position[i] = rospy.get_param(
                'atlas_controller/gains/' + name[7::] + '/i')
            self._command.kd_position[i] = self._kd_position[i]
            self._command.i_effort_max[i] = rospy.get_param(
                'atlas_controller/gains/' + name[7::] + '/i_clamp')
            self._command.i_effort_min[i] = -self._command.i_effort_max[i]

        rospy.Subscriber("/atlas/joint_states", JointState,
                         self._jointStatesCallback)
        rospy.Subscriber("/atlas/imu", Imu, self._OrientationCallback)
        self._pub = rospy.Publisher('/atlas/atlas_command', AtlasCommand)
        rospy.sleep(0.5)
コード例 #4
0
def go():
    pub = rospy.Publisher('atlas/atlas_command', AtlasCommand)
    while not rospy.is_shutdown():
        ac = AtlasCommand()
        # Wiggle it, just a little bit
        pos = 0.1 * math.sin(time.time())
        for i in range(0, NUM_JOINTS):
            ac.position.append(pos)
            ac.velocity.append(0.0)
            ac.effort.append(0.0)
        pub.publish(ac)
        time.sleep(0.1)
コード例 #5
0
ファイル: drc2_tools.py プロジェクト: alonzanbar/robil
    def __init__(self):

        self.Joint_dict = {AtlasCommand_msg_handler.atlasJointNames[i]:i for i in xrange(len(AtlasCommand_msg_handler.atlasJointNames))}
        self.inv_dict = dict(zip(self.Joint_dict.values(), self.Joint_dict.keys()))

        if rospy.get_name() == '/unnamed':
            rospy.init_node('AtlasCommand_msg_handler',anonymous=True)
        self._ac_pub = rospy.Publisher('atlas/atlas_command', AtlasCommand)
        self._command = AtlasCommand()
        self._joint_names = list(self.atlasJointNames)
        self._command.k_effort = [255] * len(self._joint_names)
        self.set_default_gains_from_param()
        self.reset_command()
コード例 #6
0
 def __init__(self, robot_name, robot_joints):
     self.RobotName = robot_name
     self.JointNames = robot_joints
     self.Joint_dict = {
         self.JointNames[i]: i
         for i in xrange(len(self.JointNames))
     }
     self.inv_dict = dict(
         zip(self.Joint_dict.values(), self.Joint_dict.keys()))
     if rospy.get_name() == '/unnamed':
         rospy.init_node('JointCommands_msg_handler', anonymous=True)
     self._compub = rospy.Publisher('/' + self.RobotName + '/atlas_command',
                                    AtlasCommand)
     self._command = AtlasCommand()
     self.set_default_gains_from_param()
     self.reset_command()
コード例 #7
0
    def demo(self):
        # Step 1: Go to stand-prep pose under user control
        stand_prep_msg = AtlasCommand()
        # Always insert current time
        stand_prep_msg.header.stamp = rospy.Time.now()
        # Assign some position and gain values that will get us there.
        stand_prep_msg.position = [
            2.438504816382192e-05, 0.0015186156379058957,
            9.983908967114985e-06, -0.0010675729718059301,
            -0.0003740221436601132, 0.06201673671603203, -0.2333149015903473,
            0.5181407332420349, -0.27610817551612854, -0.062101610004901886,
            0.00035181696875952184, -0.06218484416604042, -0.2332201600074768,
            0.51811283826828, -0.2762000858783722, 0.06211360543966293,
            0.29983898997306824, -1.303462266921997, 2.0007927417755127,
            0.49823325872421265, 0.0003098883025813848, -0.0044272784143686295,
            0.29982614517211914, 1.3034454584121704, 2.000779867172241,
            -0.498238742351532, 0.0003156556049361825, 0.004448802210390568
        ]
        stand_prep_msg.velocity = [0.0] * self.NUM_JOINTS
        stand_prep_msg.effort = [0.0] * self.NUM_JOINTS
        stand_prep_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Going to stand prep position...')
        self.ac_pub.publish(stand_prep_msg)
        time.sleep(2.0)

        # Step 2: Request BDI stand mode
        stand_msg = AtlasSimInterfaceCommand()
        # Always insert current time
        stand_msg.header.stamp = rospy.Time.now()
        # Tell it to stand
        stand_msg.behavior = stand_msg.STAND_PREP
        # Set k_effort = [255] to indicate that we still want all joints under user
        # control.  The stand behavior needs a few iterations before it start
        # outputting force values.
        stand_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Warming up BDI stand...')
        self.asic_pub.publish(stand_msg)
        time.sleep(1.0)
        # Now switch to stand
        stand_msg.behavior = stand_msg.STAND
        # Set k_effort = [0] to indicate that we want all joints under BDI control
        stand_msg.k_effort = [0] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[BDI] Standing...')
        self.asic_pub.publish(stand_msg)
        time.sleep(2.0)

        # Step 3: Move the arms and head a little (not too much; don't want to fall
        # over)
        slight_movement_msg = AtlasCommand()
        # Always insert current time
        slight_movement_msg.header.stamp = rospy.Time.now()
        # Start with 0.0 and set values for the joints that we want to control
        slight_movement_msg.position = [0.0] * self.NUM_JOINTS
        slight_movement_msg.position[AtlasState.neck_ry] = -0.1
        slight_movement_msg.position[AtlasState.l_arm_ely] = 2.1
        slight_movement_msg.position[AtlasState.l_arm_wrx] = -0.1
        slight_movement_msg.position[AtlasState.r_arm_ely] = 2.1
        slight_movement_msg.position[AtlasState.r_arm_wrx] = -0.1
        slight_movement_msg.velocity = [0.0] * self.NUM_JOINTS
        slight_movement_msg.effort = [0.0] * self.NUM_JOINTS
        slight_movement_msg.kp_position = [
            20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0,
            300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0,
            200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0,
            100.0
        ]
        slight_movement_msg.ki_position = [0.0] * self.NUM_JOINTS
        slight_movement_msg.kd_position = [0.0] * self.NUM_JOINTS
        # Bump up kp_velocity to reduce the jerkiness of the transition
        stand_prep_msg.kp_velocity = [50.0] * self.NUM_JOINTS
        slight_movement_msg.i_effort_min = [0.0] * self.NUM_JOINTS
        slight_movement_msg.i_effort_max = [0.0] * self.NUM_JOINTS
        # Set k_effort = [1] for the joints that we want to control.
        # BDI has control of the other joints
        slight_movement_msg.k_effort = [0] * self.NUM_JOINTS
        slight_movement_msg.k_effort[AtlasState.neck_ry] = 255
        slight_movement_msg.k_effort[AtlasState.l_arm_ely] = 255
        slight_movement_msg.k_effort[AtlasState.l_arm_wrx] = 255
        slight_movement_msg.k_effort[AtlasState.r_arm_ely] = 255
        slight_movement_msg.k_effort[AtlasState.r_arm_wrx] = 255
        # Publish and give time to take effect
        print('[USER/BDI] Command neck and arms...')
        self.ac_pub.publish(slight_movement_msg)
        time.sleep(2.0)

        # Step 4: Request BDI walk
        walk_msg = AtlasSimInterfaceCommand()
        # Always insert current time
        walk_msg.header.stamp = rospy.Time.now()
        # Tell it to walk
        walk_msg.behavior = walk_msg.WALK
        walk_msg.walk_params.use_demo_walk = False
        # Fill in some steps
        for i in range(4):
            step_data = AtlasBehaviorStepData()
            # Steps are indexed starting at 1
            step_data.step_index = i + 1
            # 0 = left, 1 = right
            step_data.foot_index = i % 2
            # 0.3 is a good number
            step_data.swing_height = 0.3
            # 0.63 is a good number
            step_data.duration = 0.63
            # We'll specify desired foot poses in ego-centric frame then
            # transform them into the robot's world frame.
            # Match feet so that we end with them together
            step_data.pose.position.x = (1 + i / 2) * 0.25
            # Step 0.15m to either side of center, alternating with feet
            step_data.pose.position.y = 0.15 if (i % 2 == 0) else -0.15
            step_data.pose.position.z = 0.0
            # Point those feet straight ahead
            step_data.pose.orientation.x = 0.0
            step_data.pose.orientation.y = 0.0
            step_data.pose.orientation.z = 0.0
            step_data.pose.orientation.w = 1.0
            # Transform this foot pose according to robot's
            # current estimated pose in the world, which is a combination of IMU
            # and internal position estimation.
            # http://www.ros.org/wiki/kdl/Tutorials/Frame%20transformations%20%28Python%29
            f1 = posemath.fromMsg(step_data.pose)
            f2 = PyKDL.Frame(
                PyKDL.Rotation.Quaternion(self.imu_msg.orientation.x,
                                          self.imu_msg.orientation.y,
                                          self.imu_msg.orientation.z,
                                          self.imu_msg.orientation.w),
                PyKDL.Vector(self.asis_msg.pos_est.position.x,
                             self.asis_msg.pos_est.position.y,
                             self.asis_msg.pos_est.position.z))
            f = f2 * f1
            step_data.pose = posemath.toMsg(f)
            walk_msg.walk_params.step_queue[i] = step_data
        # Use the same k_effort from the last step, to retain user control over some
        # joints. BDI has control of the other joints.
        walk_msg.k_effort = slight_movement_msg.k_effort
        # Publish and give time to take effect
        print('[USER/BDI] Walking...')
        self.asic_pub.publish(walk_msg)
        time.sleep(6.0)

        # Step 5: Go back to home pose under user control
        home_msg = AtlasCommand()
        # Always insert current time
        home_msg.header.stamp = rospy.Time.now()
        # Assign some position and gain values that will get us there.
        home_msg.position = [0.0] * self.NUM_JOINTS
        home_msg.velocity = [0.0] * self.NUM_JOINTS
        home_msg.effort = [0.0] * self.NUM_JOINTS
        home_msg.kp_position = [
            20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0,
            300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0,
            200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0,
            100.0
        ]
        home_msg.ki_position = [0.0] * self.NUM_JOINTS
        home_msg.kd_position = [0.0] * self.NUM_JOINTS
        # Bump up kp_velocity to reduce the jerkiness of the transition
        home_msg.kp_velocity = [50.0] * self.NUM_JOINTS
        home_msg.i_effort_min = [0.0] * self.NUM_JOINTS
        home_msg.i_effort_max = [0.0] * self.NUM_JOINTS
        # Set k_effort = [1] to indicate that we want all joints under user control
        home_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Going to home position...')
        self.ac_pub.publish(home_msg)
        time.sleep(2.0)
コード例 #8
0
ファイル: atlas_teleop.py プロジェクト: xdiegool/drcsim
#!/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
コード例 #9
0
ファイル: demo.py プロジェクト: githubmsj1/bIped
    def demo(self):
        # Step 0: Go to home pose under user control
        home_msg = AtlasCommand()
        # Always insert current time
        home_msg.header.stamp = rospy.Time.now()
        # Assign some position and gain values that will get us there.
        home_msg.position = [0.0] * self.NUM_JOINTS
        home_msg.velocity = [0.0] * self.NUM_JOINTS
        home_msg.effort = [0.0] * self.NUM_JOINTS
        home_msg.kp_position = [20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0]
        home_msg.ki_position = [0.0] * self.NUM_JOINTS
        home_msg.kd_position = [0.0] * self.NUM_JOINTS
        # Bump up kp_velocity to reduce the jerkiness of the transition
        home_msg.kp_velocity = [50.0] * self.NUM_JOINTS
        home_msg.i_effort_min = [0.0] * self.NUM_JOINTS
        home_msg.i_effort_max = [0.0] * self.NUM_JOINTS 
        # Set k_effort = [1] to indicate that we want all joints under user control
        home_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Going to home position...')
        self.ac_pub.publish(home_msg)
        time.sleep(2.0)
    
        # Step 1: Go to stand-prep pose under user control
        stand_prep_msg = AtlasCommand()
        # Always insert current time
        stand_prep_msg.header.stamp = rospy.Time.now()
        # Assign some position and gain values that will get us there.
        stand_prep_msg.position = [2.438504816382192e-05, 0.0015186156379058957, 9.983908967114985e-06, -0.0010675729718059301, -0.0003740221436601132, 0.06201673671603203, -0.2333149015903473, 0.5181407332420349, -0.27610817551612854, -0.062101610004901886, 0.00035181696875952184, -0.06218484416604042, -0.2332201600074768, 0.51811283826828, -0.2762000858783722, 0.06211360543966293, 0.29983898997306824, -1.303462266921997, 2.0007927417755127, 0.49823325872421265, 0.0003098883025813848, -0.0044272784143686295, 0.29982614517211914, 1.3034454584121704, 2.000779867172241, -0.498238742351532, 0.0003156556049361825, 0.004448802210390568]
        stand_prep_msg.velocity = [0.0] * self.NUM_JOINTS
        stand_prep_msg.effort = [0.0] * self.NUM_JOINTS
        stand_prep_msg.kp_position = [20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0]
        stand_prep_msg.ki_position = [0.0] * self.NUM_JOINTS
        stand_prep_msg.kd_position = [0.0] * self.NUM_JOINTS
        # Bump up kp_velocity to reduce the jerkiness of the transition
        stand_prep_msg.kp_velocity = [50.0] * self.NUM_JOINTS
        stand_prep_msg.i_effort_min = [0.0] * self.NUM_JOINTS
        stand_prep_msg.i_effort_max = [0.0] * self.NUM_JOINTS 
        # Set k_effort = [1] to indicate that we want all joints under user control
        stand_prep_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Going to stand prep position...')
        self.ac_pub.publish(stand_prep_msg)
        time.sleep(2.0)
    
        # Step 2: Request BDI stand mode
        stand_msg = AtlasSimInterfaceCommand()
        # Always insert current time
        stand_msg.header.stamp = rospy.Time.now()
        # Tell it to stand
        stand_msg.behavior = stand_msg.STAND_PREP
        # Set k_effort = [255] to indicate that we still want all joints under user
        # control.  The stand behavior needs a few iterations before it start
        # outputting force values.
        stand_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Warming up BDI stand...')
        self.asic_pub.publish(stand_msg)
        time.sleep(1.0)
        # Now switch to stand
        stand_msg.behavior = stand_msg.STAND
        # Set k_effort = [0] to indicate that we want all joints under BDI control
        stand_msg.k_effort = [0] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[BDI] Standing...')
        self.asic_pub.publish(stand_msg)
        time.sleep(2.0)
     
        # Step 3: Move the arms and head a little (not too much; don't want to fall
        # over)
        slight_movement_msg = AtlasCommand()
        # Always insert current time
        slight_movement_msg.header.stamp = rospy.Time.now()
        # Start with 0.0 and set values for the joints that we want to control
        slight_movement_msg.position = [0.0] * self.NUM_JOINTS
        slight_movement_msg.position[AtlasState.neck_ay] = -0.1
        slight_movement_msg.position[AtlasState.l_arm_ely] = 2.1
        slight_movement_msg.position[AtlasState.l_arm_mwx] = -0.1
        slight_movement_msg.position[AtlasState.r_arm_ely] = 2.1
        slight_movement_msg.position[AtlasState.r_arm_mwx] = -0.1
        slight_movement_msg.velocity = [0.0] * self.NUM_JOINTS
        slight_movement_msg.effort = [0.0] * self.NUM_JOINTS
        slight_movement_msg.kp_position = [20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0]
        slight_movement_msg.ki_position = [0.0] * self.NUM_JOINTS
        slight_movement_msg.kd_position = [0.0] * self.NUM_JOINTS
        # Bump up kp_velocity to reduce the jerkiness of the transition
        stand_prep_msg.kp_velocity = [50.0] * self.NUM_JOINTS
        slight_movement_msg.i_effort_min = [0.0] * self.NUM_JOINTS
        slight_movement_msg.i_effort_max = [0.0] * self.NUM_JOINTS 
        # Set k_effort = [1] for the joints that we want to control.
        # BDI has control of the other joints
        slight_movement_msg.k_effort = [0] * self.NUM_JOINTS
        slight_movement_msg.k_effort[AtlasState.neck_ay] = 255
        slight_movement_msg.k_effort[AtlasState.l_arm_ely] = 255
        slight_movement_msg.k_effort[AtlasState.l_arm_mwx] = 255
        slight_movement_msg.k_effort[AtlasState.r_arm_ely] = 255
        slight_movement_msg.k_effort[AtlasState.r_arm_mwx] = 255
        # Publish and give time to take effect
        print('[USER/BDI] Command neck and arms...')
        self.ac_pub.publish(slight_movement_msg)
        time.sleep(2.0)
    
        # Step 4: Request BDI walk
        walk_msg = AtlasSimInterfaceCommand()
        # Always insert current time
        walk_msg.header.stamp = rospy.Time.now()
        # Tell it to walk
        walk_msg.behavior = walk_msg.WALK
        walk_msg.walk_params.use_demo_walk = False
        # Fill in some steps
        for i in range(4):
            step_data = AtlasBehaviorStepData()
            # Steps are indexed starting at 1
            step_data.step_index = i+1
            # 0 = left, 1 = right
            step_data.foot_index = i%2
            # 0.3 is a good number
            step_data.swing_height = 0.3
            # 0.63 is a good number
            step_data.duration = 0.63
            # We'll specify desired foot poses in ego-centric frame then
            # transform them into the robot's world frame.
            # Match feet so that we end with them together
            step_data.pose.position.x = (1+i/2)*0.25
            # Step 0.15m to either side of center, alternating with feet
            step_data.pose.position.y = 0.15 if (i%2==0) else -0.15
            step_data.pose.position.z = 0.0
            # Point those feet straight ahead
            step_data.pose.orientation.x = 0.0
            step_data.pose.orientation.y = 0.0
            step_data.pose.orientation.z = 0.0
            step_data.pose.orientation.w = 1.0
            # Transform this foot pose according to robot's
            # current estimated pose in the world, which is a combination of IMU
            # and internal position estimation.
            # http://www.ros.org/wiki/kdl/Tutorials/Frame%20transformations%20%28Python%29
            f1 = posemath.fromMsg(step_data.pose)
            f2 = PyKDL.Frame(PyKDL.Rotation.Quaternion(self.imu_msg.orientation.x,
                                                       self.imu_msg.orientation.y,
                                                       self.imu_msg.orientation.z,
                                                       self.imu_msg.orientation.w),
                             PyKDL.Vector(self.asis_msg.pos_est.position.x,
                                          self.asis_msg.pos_est.position.y,
                                          self.asis_msg.pos_est.position.z))
            f = f2 * f1
            step_data.pose = posemath.toMsg(f)
            walk_msg.walk_params.step_queue[i] = step_data
        # Use the same k_effort from the last step, to retain user control over some
        # joints. BDI has control of the other joints.
        walk_msg.k_effort = slight_movement_msg.k_effort
        # Publish and give time to take effect
        print('[USER/BDI] Walking...')
        self.asic_pub.publish(walk_msg)
        time.sleep(6.0)

        # Step 5: Go back to home pose under user control
        home_msg = AtlasCommand()
        # Always insert current time
        home_msg.header.stamp = rospy.Time.now()
        # Assign some position and gain values that will get us there.
        home_msg.position = [0.0] * self.NUM_JOINTS
        home_msg.velocity = [0.0] * self.NUM_JOINTS
        home_msg.effort = [0.0] * self.NUM_JOINTS
        home_msg.kp_position = [20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0]
        home_msg.ki_position = [0.0] * self.NUM_JOINTS
        home_msg.kd_position = [0.0] * self.NUM_JOINTS
        # Bump up kp_velocity to reduce the jerkiness of the transition
        home_msg.kp_velocity = [50.0] * self.NUM_JOINTS
        home_msg.i_effort_min = [0.0] * self.NUM_JOINTS
        home_msg.i_effort_max = [0.0] * self.NUM_JOINTS 
        # Set k_effort = [1] to indicate that we want all joints under user control
        home_msg.k_effort = [255] * self.NUM_JOINTS
        # Publish and give time to take effect
        print('[USER] Going to home position...')
        self.ac_pub.publish(home_msg)
        time.sleep(2.0)
コード例 #10
0
ファイル: teleop_legs.py プロジェクト: Aharobot/drc
def init_atlas_command():

    # Initialize an Atlas command structure                               
    n = 28
    command = AtlasCommand()
    command.position    = [0] * n
    command.velocity    = [0] * n
    command.effort      = [0] * n
    command.kp_velocity = [0] * n
    command.k_effort    = [0] * n
    command.kp_position = [ rospy.get_param('atlas_controller/gains/' + name + '/p')
                            for name in ATLAS_JOINT_NAMES ]
    command.ki_position = [ rospy.get_param('atlas_controller/gains/' + name + '/i')
                            for name in ATLAS_JOINT_NAMES ]
    command.kd_position = [ rospy.get_param('atlas_controller/gains/' + name + '/d')
                            for name in ATLAS_JOINT_NAMES ]
    command.i_effort_max = [ rospy.get_param("atlas_controller/gains/" + name + "/i_clamp")
                             for name in ATLAS_JOINT_NAMES ]
    command.i_effort_min = [ -i_clamp for i_clamp in command.i_effort_max ] 

    # Move the stupid head down
    command.position[3] = 0.3
    command.k_effort[3] = 255
    
    return command