Motion Control

Controlling Walking

Calling Python Module

To facilitate development, we have encapsulated the methods for controlling the robot's walking into a module. The leju_lib_pkg is the Python interface library we provide, located at ~/robot_ros_application/catkin_ws/src/leju_lib_pkg.

walk Function

def walk(self):
    if self.__check_id() != True:
        return False
    status = self.__get_status()
    if status == None:
        return -1
    if status.data == 'walking':
        return True
    if self.ready() == True:
        result = self.__set_status(self.ctl_id, 'walking')
        if result == None:
            return -2
        if result == self.__state_walking:
            return True
    return False

• Before walking, you need to call this method to set the robot's status to walking mode; otherwise, the robot will not execute the walking command.

walking Function

# path: ~/robot_ros_application/catkin_ws/src/leju_lib_pkg/src/motion/bodyhub_client.py
def walking(self, delta_x, delta_y, theta, timeout = None):
    try:
        rospy.wait_for_message('/requestGaitCommand', Bool, self.__timeout if timeout is None else timeout)
    except:
        rospy.logwarn("wait_for_service requestGaitCommand timeout!")
        result = 1
    else:
        result = 0
    self.__is_walking = 1
    self.__gait_cmd_pub.publish(data=[delta_x, delta_y, theta])
    return result

• This is the lowest-level walking encapsulation, with higher-level functions based on this function.

• The walking function directly sends single-step information to /requestGaitCommand, including delta_x (forward/backward), delta_y (left/right), and theta (rotation).

• Following the right-hand rule, moving forward, to the left, and counterclockwise are positive directions, while the opposite directions are negative.

• If the delta_x, delta_y, or theta parameters are unreasonable (exceeding the robot's limits), an inverse kinematics failure warning (ik failed!!!) will occur, and the robot will not execute this call. Therefore, it is not recommended for developers to directly call this function. Instead, use the following functions, which limit the robot's maximum step size.

walking_the_distance Function

# path: ~/robot_ros_application/catkin_ws/src/leju_lib_pkg/src/motion/bodyhub_client.py
def walking_the_distance(self, delta_x, delta_y, theta):
    direction = [0, 0, 0]
    direction[0] = 1 if delta_x >= 0 else -1
    direction[1] = 1 if delta_y >= 0 else -1
    direction[2] = 1 if theta >= 0 else -1

    xNumberOfStep = abs(delta_x)//self.__step_len_max[0]
    yNumberOfStep = abs(delta_y)//self.__step_len_max[1]
    aNumberOfStep = abs(theta)//self.__step_len_max[2]
    delta_x = delta_x - ((self.__step_len_max[0] * xNumberOfStep)*direction[0])
    delta_y = delta_y - ((self.__step_len_max[1] * yNumberOfStep)*direction[1])
    theta = theta - ((self.__step_len_max[2] * aNumberOfStep)*direction[2])
    numberOfStep = max(xNumberOfStep, yNumberOfStep, aNumberOfStep)

    for i in range(0, int(numberOfStep+1)):
        if xNumberOfStep >= 1:
            x = self.__step_len_max[0]*direction[0]
            xNumberOfStep = xNumberOfStep - 1
        elif xNumberOfStep == 0:
            x = delta_x
            xNumberOfStep = xNumberOfStep - 1
        else:
            x = 0.0

        if yNumberOfStep >= 1:
            y = self.__step_len_max[1]*direction[1]
            yNumberOfStep = yNumberOfStep - 1
        elif yNumberOfStep == 0:
            y = delta_y
            yNumberOfStep = yNumberOfStep - 1
        else:
            y = 0.0

        if aNumberOfStep >= 1:
            a = self.__step_len_max[2]*direction[2]
            aNumberOfStep = aNumberOfStep - 1
        elif aNumberOfStep == 0:
            a = theta
            aNumberOfStep = aNumberOfStep - 1
        else:
            a = 0.0
        self.walking(x, y, a)

• This function allows users to input the distance to walk and the angle of rotation. The function divides the distance into steps based on the robot's maximum step size. The parameters are the same as for the walking function, but in this function, you can directly input the distance to the target point.

• The maximum step size is limited in the self.__step_len_max attribute to avoid inverse kinematics failures.

walking_n_steps Function

# path: ~/robot_ros_application/catkin_ws/src/leju_lib_pkg/src/motion/bodyhub_client.py
def walking_n_steps(self, cmd, n):
    for i in range(0, n):
        if self.walking(cmd[0], cmd[1], cmd[2]):
            return 1
    return 0

• This function allows users to input a single step size and the number of steps, but the step size must be limited when input.

• The cmd parameter is a list, defined as follows:

  • @cmd: [delta_x, delta_y, theta]

  • The parameters are the same as for the walking function, essentially making it a walking function that can input the number of steps.

• The n parameter is the number of steps, which should be greater than 0.

Python Example

#!/usr/bin python
# -*- coding: utf-8 -*-
import rospy
import rospkg
import sys

sys.path.append(rospkg.RosPack().get_path('leju_lib_pkg'))
import motion.bodyhub_client as bodycli

CTL_ID = 2

class Walking(bodycli.BodyhubClient):
    def __init__(self):
        super(Walking, self).__init__(CTL_ID)

    def do_some_walking(self):
        # Walk in an S-shaped curve

        # Method 1: Call the walking function
        for i in range(0, 5):
            self.walking(0.0, 0.0, 12.0)
        for i in range(0, 16):
            self.walking(0.07, 0.0, -7.5)
        for i in range(0, 16):
            self.walking(0.07, 0.0, 7.5)
        for i in range(0, 5):
            self.walking(0.0, 0.0, -12.0)

        # Method 2: Call the walking_the_distance function
        # self.walking_the_distance(0.0, 0.0, 60.0)
        # self.walking_the_distance(1.12, 0.0, -120.0)
        # self.walking_the_distance(1.12, 0.0, 120.0)
        # self.walking_the_distance(0.0, 0.0, -60.0)

        # Method 3: Call the walk_n_step function
        # self.walk_n_step([0.0, 0.0, 12.0], 5)
        # self.walk_n_step([0.07, 0.0, -7.5], 16)
        # self.walk_n_step([0.07, 0.0, 7.5], 16)
        # self.walk_n_step([0.0, 0.0, -12.0], 5)

        self.wait_walking_done()

    def start(self):
        self.walk()
        self.do_some_walking()
        rospy.signal_shutdown('exit')

    def rosShutdownHook(self):
        self.reset()

if __name__ == "__main__":
    rospy.init_node('walking_node', anonymous=True)
    walking = Walking()
    rospy.on_shutdown(walking.rosShutdownHook)
    walking.start()

Controlling End-Effector Trajectories

Calling Python Module

• The control of end-effector trajectories is achieved through the ikmodule inverse kinematics module, defined in ~/robot_ros_application/catkin_ws/src/leju_lib_pkg/src/ik_lib/ikmodulesim/ikmodulesim.py. Below are the interface functions for calling:

toInitPoses Function

• Function Definition：

  def toInitPoses(self):
      if self.setstatus() and self.getposes():
          self.PosPara_wF.Torso_z = -self.leftLegZ
          self.PosPara_wF.Lfoot_y = self.leftLegY
          self.PosPara_wF.Rfoot_y = self.rightLegY
          self.PosPara_wF.RArm_x = self.rightArmX
          self.PosPara_wF.RArm_y = self.rightArmY
          self.PosPara_wF.RArm_z = self.rightArmZ
          self.PosPara_wF.LArm_x = self.leftArmX
          self.PosPara_wF.LArm_y = self.leftArmY
          self.PosPara_wF.LArm_z = self.leftArmZ
          return True
      return False

• Initialize the ikmodule, which must be called before body_motion.

• Returns True on successful initialization, otherwise False.

End-Effector Positions

• The end-effector positions are defined in the CtrlType class in ~/robot_ros_application/catkin_ws/src/leju_lib_pkg/src/ik_lib/ikmodulesim/CtrlType.py. These positions are used in the body_motion function to specify the end-effectors to be controlled.

• The following end-effectors are defined:

  Definition	Meaning
  Torso	Center of mass of the torso
  Torso_R	Rotation of the torso around the x-axis, counterclockwise is positive
  Torso_P	Rotation of the torso around the y-axis, counterclockwise is positive
  Torso_Y	Rotation of the torso around the z-axis, counterclockwise is positive
  Torso_x	Displacement of the torso along the x-axis, forward is positive
  Torso_y	Displacement of the torso along the y-axis, left is positive
  Torso_z	Displacement of the torso along the z-axis, upward is positive
  Lfoot	Left foot
  Lfoot_R	Rotation of the left foot around the x-axis, counterclockwise is positive
  Lfoot_P	Rotation of the left foot around the y-axis, counterclockwise is positive
  Lfoot_Y	Rotation of the left foot around the z-axis, counterclockwise is positive
  Lfoot_x	Displacement of the left foot along the x-axis, forward is positive
  Lfoot_y	Displacement of the left foot along the y-axis, left is positive
  Lfoot_z	Displacement of the left foot along the z-axis, upward is positive
  Rfoot	Right foot
  Rfoot_...	Similar to the left foot
  LArm_x	Displacement of the left arm along the x-axis, forward is positive
  LArm_y	Displacement of the left arm along the y-axis, left is positive
  LArm_z	Displacement of the left arm along the z-axis, upward is positive
  RArm_...	Similar to the left arm

body_motion Function

• Function Definition：

  def body_motion(self, body_pose, value, count=100):
      """
      Simplified call
      Args:
          - body_pose: CtrlType, or [CtrlType]
          - value: list, or two dim list. RPY, xyz.
          - count: int. times of division
  
      """
      if not (isinstance(body_pose, list) or isinstance(body_pose, tuple)):
          body_pose = [body_pose]
          value = [value]
  
      d = {}
  
      for t, diff in zip(body_pose, value):
          if isinstance(t.key(), str):
              d[t.key()] = diff
          else:
              for key, dif in zip(t.key(), diff):
                  d[key] = dif
  
      for index in range(count):
          for k, v in d.items():
              setattr(self.PosPara_wF, k, getattr(self.PosPara_wF, k) + v * 1.0 / count)
  
          poseArrayMsg = PoseArray()
          poseArrayMsg.poses.append(self.getleftlegPosMsg())
          poseArrayMsg.poses.append(self.getrightlegPosMsg())
          poseArrayMsg.poses.append(self.getleftarmPosMsg())
          poseArrayMsg.poses.append(self.getrightarmPosMsg())
          poseArrayMsg.controlId = 6
          self.targetPosesPub.publish(poseArrayMsg)
  
      rospy.loginfo("waitPostureDone...")
      self.waitPostureDone()

  • The robot's end-effector motion execution involves specifying the target posture body_pose and calculating the servo angles through inverse kinematics to control the robot's movement.

  • This function requires three parameters:

    • body_pose

      • The end-effector position to be controlled. You can pass a single one, or if multiple are needed, they should be passed in a list, such as:

        body_motion(end_effector_position, ...) 或 body_motion([end_effector_position1, end_effector_position2], ...)

    • value

      • Represents the posture values for the corresponding end-effectors. Each end-effector position should correspond to a posture value, such as:

        body_motion([end_effector_position1, end_effector_position2], [posture_value1, posture_value2], ...)

      • The definition of posture values

        • If it is Torso, Lfoot, or Rfoot, the posture value is a list, defined as follows:

          [x, y, z, R, P, Y]

        • If it specifies an axis rotation or displacement, such as Torso_y, only the displacement along the y-axis of the torso needs to be passed.

      • The units of posture values

        • Displacement units are in meters (m), and rotation units are in degrees (°).

    • count

      • Number of interpolations.

reset Function

• Function Definition：

  def reset(self):
      rospy.wait_for_service("MediumSize/IKmodule/SetStatus", 30)
      try:
          resp = self.setStatusClient.call(6, "reset")
          rospy.loginfo("Set status with response: %d" % resp.stateRes)
          return True
      except rospy.ServiceException as e:
          rospy.logwarn("Reset status service call failed: %s" % e)

• Used for resetting after action execution is complete.

Python Example

#!/usr/local/bin/python
# -*- coding: utf-8 -*-
import rospy
import math

from ik_lib.ikmodulesim import IkModuleSim
from ik_lib.ikmodulesim.CtrlType import CtrlType as C
from mediumsize_msgs.srv import *

class IkAction(IkModuleSim):
    def __init__(self):
        super(IkAction, self).__init__()                # Inherit from IkModuleSim

    def do_some_action(self):
        if self.toInitPoses() == True:                  # Initialize Ik
            self.body_motion(C.Torso, [0, 0, -0.04, 0, 0, 0])               # The center of mass of the torso moves 4cm downward along the z-axis, i.e., the robot squats 4cm
            self.body_motion([C.Torso_x, C.Torso_P], [0.02, math.pi/18])    # The center of mass of the torso moves 2cm forward along the x-axis and rotates forward by Π / 18° around the y-axis
            self.body_motion(C.Torso_z, 0.02)                               # The center of mass of the torso moves 2cm upward along the z-axis
            self.body_motion([C.LArm_x, C.RArm_z], [0.04, 0.04])            # The left hand moves 4cm forward along the x-axis, and the right hand moves 4cm upward along the z-axis
        else:
            rospy.logerr("ik_lib to initpose failed")

    def rosShutdownHook(self):
        self.reset()

if __name__ == "__main__":
    rospy.init_node("IkAction")
    IKA = IkAction()
    rospy.on_shutdown(IKA.rosShutdownHook)
    IKA.do_some_action()

• Before running this script, you should load the workspace environment variables in the terminal where you are running the script, and then run the Python script:

  source ~/robot_ros_application/catkin_ws/devel/setup.bash