soccer_pycontrol.soccerbot_ros.SoccerbotRos

class soccer_pycontrol.soccerbot_ros.SoccerbotRos(pose: Transformation, useFixedBase=False, useCalibration=True)[source]

Bases: Soccerbot

The main class for the robot, which receives and sends information to ROS

__init__(pose: Transformation, useFixedBase=False, useCalibration=True)[source]

Initialization function for soccerbot. Does a series of calculations based on the URDF file for standing, walking poses

Parameters:

pose – The position to initialize the robot, Z doesn’t matter here as it will be set automatically
useFixedBase – Whether to fix the base_link in the air for movement testing
useCalibration – Whether to use calibration for walking path calculations

Methods

`__init__`(pose[, useFixedBase, useCalibration])	Initialization function for soccerbot.
`apply_foot_pressure_sensor_feedback`(floor)	Add foot pressure sensor feedback (Currently not implemented)
`apply_head_rotation`()	Does head rotation for the robot, robot will try to face the ball if it is ready, otherwise rotate around if its relocalizing or finding the ball
`apply_imu_feedback`(imu_pose)	Adds IMU feedback while the robot is moving to the arms
`apply_imu_feedback_standing`(imu_pose)	Adds IMU feedback while the robot is standing or getting ready to the arms
`apply_phase_difference_roll_feedback`(t, imu_pose)	Adds IMU feedback while the robot is moving to the arms
`ball_pixel_callback`(msg)	Callback function for ball pixel, used for head rotation calculations
`createPathToGoal`(endPose)	Creates a path from the robot's current location to the goal location
`get_angles`()	Function for getting all the angles, combines the configuration with the configuration offset
`get_foot_pressure_sensors`(floor)	Checks if 4 corners of the each feet are in contact with ground #TODO fix docstring
`get_imu`()	Gets the IMU at the IMU link location.
`get_imu_raw`([verbose])	Simulates the IMU at the IMU link location.
`get_link_transformation`(link1, link2)	Gives the H-trasnform between two links :param link1: Starting link :param link2: Ending link :return: H-transform from starting link to the ending link
`get_phase_difference_roll`(t, imu_pose)
`imu_callback`(msg)	Callback function for IMU information
`inverseKinematicsLeftFoot`(transformation)	Inverse kinematic function for the left foot.
`inverseKinematicsRightFoot`(transformation)	# Does the inverse kinematics calculation for the right foot
`plot_angles`()	Creates a plot of all the angles
`publishAngles`()	Send the robot angles based on self.configuration + self.configuration_offset to ROS
`publishOdometry`(time)	Send the odometry of the robot to be used in localization by ROS
`publishPath`([robot_path])	Publishes the robot path to rviz for debugging and visualization
`ready`()	Sets the robot's joint angles for the robot to standing pose.
`reset_imus`()	Reset the walking and standing PID values
`reset_roll_feedback_parameters`()
`setPose`(pose)	Teleports the robot to the desired pose
`setWalkingTorsoHeight`(pose)	Takes a 2D pose and sets the height of the pose to the height of the torso https://docs.google.com/presentation/d/10DKYteySkw8dYXDMqL2Klby-Kq4FlJRnc4XUZyJcKsw/edit#slide=id.g163c1c67b73_0_0
`state_callback`(robot_state)	Callback function for information about the robot's state
`stepPath`(t)	Updates the configuration for the robot for the next position t based on the current path
`updateRobotConfiguration`()	Reads the joint_states message and resets all the positions of all the joints

Attributes

`cleats_offset`	Additional height added by cleats and grass, consists of 1cm grass and 0.5cm cleats
`head_yaw_freq`	Frequency for the head's yaw while searching and relocalizing (left and right movement)
`head_pitch_freq`	Frequency for the head's while searching and relocalizing yaw (up and down movement)
`walking_torso_height`	Height of the robot's torso (center between two arms) while walking
`foot_box`	Dimensions of the foot collision box #TODO get it from URDF
`right_foot_joint_center_to_collision_box_center`	//docs.google.com/presentation/d/10DKYteySkw8dYXDMqL2Klby-Kq4FlJRnc4XUZyJcKsw/edit#slide=id.g163c1c67b73_0_0)
`arm_0_center`	Ready Pose angle for arm 1
`arm_1_center`	Ready Pose angle for arm 2
`configuration`	The 18x1 float array motor angle configuration for the robot's 18 motors
`configuration_offset`	The offset for the 18x1 motor angle configurations
`standing_pid`	PID values to adjust the torso's front and back movement while standing, getting ready to walk, and post walk
`walking_pid`	PID values to adjust the torso's front and back movement while walking
`odom_pose_start_path`	Odom pose at start of path, reset everytime a new path is created Odom pose, always starts at (0,0) and is the odometry of the robot's movement.

apply_foot_pressure_sensor_feedback(floor)

Add foot pressure sensor feedback (Currently not implemented)

Parameters:: floor – The floor object

apply_head_rotation()[source]: Does head rotation for the robot, robot will try to face the ball if it is ready, otherwise rotate around if its relocalizing or finding the ball

apply_imu_feedback(imu_pose: Transformation)

Adds IMU feedback while the robot is moving to the arms

Parameters:: imu_pose – Pose of the torso
Returns:: The value for the walking_pid controller

apply_imu_feedback_standing(imu_pose: Transformation)

Adds IMU feedback while the robot is standing or getting ready to the arms

Parameters:: imu_pose – Pose of the torso
Returns:: The value for the walking_pid controller

apply_phase_difference_roll_feedback(t, imu_pose: Transformation)

Adds IMU feedback while the robot is moving to the arms

Parameters:: imu_pose – Pose of the torso
Returns:: The value for the walking_pid controller

arm_0_center: Ready Pose angle for arm 1

arm_1_center: Ready Pose angle for arm 2

ball_pixel_callback(msg: Pose2D)[source]

Callback function for ball pixel, used for head rotation calculations

Parameters:: msg – The location of the pixel of the center of the ball in the Camera

cleats_offset: Additional height added by cleats and grass, consists of 1cm grass and 0.5cm cleats

configuration: The 18x1 float array motor angle configuration for the robot’s 18 motors

configuration_offset: The offset for the 18x1 motor angle configurations

createPathToGoal(endPose: Transformation) → PathRobot

Creates a path from the robot’s current location to the goal location

Parameters:: endPose – 3D transformation
Returns:: Robot path

foot_box: Dimensions of the foot collision box #TODO get it from URDF

get_angles()

Function for getting all the angles, combines the configuration with the configuration offset

Returns:: All 18 angles of the robot in an array formation

get_foot_pressure_sensors(floor)[source]

Checks if 4 corners of the each feet are in contact with ground #TODO fix docstring

Indices for looking from above on the feet plates
Left         Right
4——-5    0——-1
|   ^   |    |   ^   |      ^
|   |   |    |   |   |      | forward direction
|       |    |       |
6——-7    2——-3

Parameters:: floor – PyBullet body id of the plane the robot is walking on.
Returns:: boolean array of 8 contact points on both feet, True: that point is touching the ground False: otherwise

get_imu()[source]

Gets the IMU at the IMU link location.

Returns:: calculated orientation of the center of the torso of the robot

get_imu_raw(verbose=False)

Simulates the IMU at the IMU link location. TODO: Add noise model, make the refresh rate vary (currently in sync with the PyBullet time steps)

Parameters:: verbose – Optional - Set to True to print the linear acceleration and angular velocity
Returns:: concatenated 3-axes values for linear acceleration and angular velocity

get_link_transformation(link1, link2): Gives the H-trasnform between two links :param link1: Starting link :param link2: Ending link :return: H-transform from starting link to the ending link

head_pitch_freq: Frequency for the head’s while searching and relocalizing yaw (up and down movement)

head_yaw_freq: Frequency for the head’s yaw while searching and relocalizing (left and right movement)

imu_callback(msg: Imu)[source]

Callback function for IMU information

Parameters:: msg – IMU Message

inverseKinematicsLeftFoot(transformation)

Inverse kinematic function for the left foot. Works due to symmetry between left and right foot.

Parameters:: transformation – The 3D transformation from the torso center to the foot center
Returns:: Motor angles for the left foot

inverseKinematicsRightFoot(transformation)

# Does the inverse kinematics calculation for the right foot

Parameters:: transformation – The 3D transformation from the torso center to the foot center
Returns:: 6x1 Motor angles for the right foot

odom_pose_start_path: Odom pose at start of path, reset everytime a new path is created Odom pose, always starts at (0,0) and is the odometry of the robot’s movement. All odom paths start from odom pose

plot_angles(): Creates a plot of all the angles

publishAngles()[source]: Send the robot angles based on self.configuration + self.configuration_offset to ROS

publishOdometry(time: Time)[source]: Send the odometry of the robot to be used in localization by ROS

publishPath(robot_path=None)[source]

Publishes the robot path to rviz for debugging and visualization

Parameters:: robot_path – The path to publish, leave empty to publish the robot’s current path

ready() → None: Sets the robot’s joint angles for the robot to standing pose.

reset_imus(): Reset the walking and standing PID values

right_foot_joint_center_to_collision_box_center

//docs.google.com/presentation/d/10DKYteySkw8dYXDMqL2Klby-Kq4FlJRnc4XUZyJcKsw/edit#slide=id.g163c1c67b73_0_0)

Type:: Transformations from the right foots joint position to the center of the collision box of the foot (https

setPose(pose: Transformation)

Teleports the robot to the desired pose

Parameters:: pose – 3D position in pybullet

setWalkingTorsoHeight(pose: Transformation) → Transformation

Takes a 2D pose and sets the height of the pose to the height of the torso https://docs.google.com/presentation/d/10DKYteySkw8dYXDMqL2Klby-Kq4FlJRnc4XUZyJcKsw/edit#slide=id.g163c1c67b73_0_0

Parameters:: position – 2D position of the robot’s torso in a 3D transformation format

standing_pid: PID values to adjust the torso’s front and back movement while standing, getting ready to walk, and post walk

state_callback(robot_state: RobotState)[source]

Callback function for information about the robot’s state

Parameters:: robot_state – A class which tells you information about the robot, disconnected etc

stepPath(t)[source]

Updates the configuration for the robot for the next position t based on the current path

Parameters:: t – Timestep relative to the time of the first path, where t=0 is the beginning of the path

updateRobotConfiguration() → None[source]: Reads the joint_states message and resets all the positions of all the joints

walking_pid: PID values to adjust the torso’s front and back movement while walking

walking_torso_height: Height of the robot’s torso (center between two arms) while walking