Atlas Sim Interface
1.0
|
The primary interface for the DRC version of the Atlas Robot. More...
#include <AtlasSimInterface.h>
The primary interface for the DRC version of the Atlas Robot.
Public Member Functions | |
int | get_version_major () |
return major version number | |
int | get_version_minor () |
return minor version number | |
int | get_version_point () |
return point version number | |
AtlasErrorCode | process_control_input (const AtlasControlInput &control_input, const AtlasRobotState &robot_state, AtlasControlOutput &control_output) |
Given control inputs and current behavior, calculate control outputs. | |
AtlasErrorCode | reset_control () |
Reset the behavior control. | |
AtlasErrorCode | set_desired_behavior (const std::string &behavior) |
Set the desired behavior of the sim. | |
AtlasErrorCode | get_desired_behavior (std::string &desired_behavior) |
Get the desired behavior of the sim. | |
AtlasErrorCode | get_current_behavior (std::string ¤t_behavior) |
Get the current behavior of the sim. | |
AtlasErrorCode | get_num_behaviors (int &num_behaviors) |
Get the number of available behaviors. | |
AtlasErrorCode | get_behavior_at_index (int index, std::string &behavior) |
Get the name of the behavior at a specified index. | |
AtlasErrorCode | get_behavior_joint_weights (const std::string &behavior, float joint_control_weights[NUM_JOINTS]) |
Get how much each joint is controlled by behavior calculations. | |
AtlasErrorCode | get_current_behavior_joint_weights (float joint_control_weights[NUM_JOINTS]) |
Get how much each joint is controlled by current behavior calculations. | |
AtlasErrorCode | get_estimated_position (AtlasPositionData &robot_pos_est, AtlasVec3f foot_pos_est[Atlas::NUM_FEET]) |
Accessor to Atlas internal position estimates. | |
std::string | get_error_code_text (AtlasErrorCode ec) |
Get string version of error code. |
return major version number
return minor version number
return point version number
AtlasErrorCode AtlasSimInterface::process_control_input | ( | const AtlasControlInput & | control_input, |
const AtlasRobotState & | robot_state, | ||
AtlasControlOutput & | control_output | ||
) |
Given control inputs and current behavior, calculate control outputs.
This function will calculate control outputs that should be sent to the Atlas Robot simulation based on values passed in in the control_input object, and previously set behavior and related settings. It is up to the caller to have set values in the control_input object based on values from a simulation.
The values in control_output can then be applied as desired to the simulation.
NOTE:* If an error is returned (e.g., the simulation is not in a behavior), the control output will not be valid, and should not be applied to the simulation.
ALSO NOTE:* Currently control outputs based on behaviors will almost always be just the desired forces, with desired positions and gains set to 0.
[in] | control_input | - control inputs |
[in] | robot_state | - current state of simulated robot |
[out] | control_output | - new desired state of sim |
Possible return values:
Reset the behavior control.
Currently calling this function is the same as calling set_desired_behavior("Freeze")
.
At some point this function should reset the control system such that a new simulation run can be started with completely reset control parameters.
AtlasErrorCode AtlasSimInterface::set_desired_behavior | ( | const std::string & | behavior | ) |
Set the desired behavior of the sim.
This function will set the *desired* behavior of the Atlas Robot simulation.
There are a number of reasons the simulation may not be able to follow the desired behavior. For example, most desired behaviors will not be achievable if the robot has fallen. Also, it may take a little while for the robot to switch from its current behavior to the desired one.
Setting the desired behavior to "User" should immediately return all joint control to the performer.
Current supported behaviors should include:
All supported behaviors can be enumerated by calling get_num_behaviors() and get_behavior_at_index().
[in] | behavior | - name of desired behavior |
AtlasErrorCode AtlasSimInterface::get_desired_behavior | ( | std::string & | desired_behavior | ) |
Get the desired behavior of the sim.
This function returns in the passed reference argument the desired behavior of the Atlas Robot simulation, as set by set_desired_behavior(). This might not be the actual behavior the robot is in; see set_desired_behavior() for more information.
[out] | desired_behavior | - string reference for returned desired behavior |
AtlasErrorCode AtlasSimInterface::get_current_behavior | ( | std::string & | current_behavior | ) |
Get the current behavior of the sim.
This function returns in the passed reference argument the current behavior of the Atlas Robot simulation. See set_desired_behavior() for information on why the desired and current behaviors may not match.
This function should be called often to know when the Atlas Robot's current behavior has changed.
[out] | current_behavior | - string reference for returned current behavior |
AtlasErrorCode AtlasSimInterface::get_num_behaviors | ( | int & | num_behaviors | ) |
Get the number of available behaviors.
Use with get_behavior_at_index() to enumberate the available behaviors.
[out] | num_behaviors | - int reference for returned number of behaviors |
AtlasErrorCode AtlasSimInterface::get_behavior_at_index | ( | int | index, |
std::string & | behavior | ||
) |
Get the name of the behavior at a specified index.
Use with get_num_behaviors() to enumberate the available behaviors.
[in] | index | of behavior |
[out] | behavior | - string reference for returned behavior name |
AtlasErrorCode AtlasSimInterface::get_behavior_joint_weights | ( | const std::string & | behavior, |
float | joint_control_weights[NUM_JOINTS] | ||
) |
Get how much each joint is controlled by behavior calculations.
This function returns how much each joint is controlled by behavior calculations for the specified behavior.
[in] | behavior | - name of behavior to check |
[out] | joint_control_weights | - array of weights of control by behavior |
For most behaviors joints can be under only total control by performers (return value 0.0), or total control by the behavior (return value 1.0).
AtlasErrorCode AtlasSimInterface::get_current_behavior_joint_weights | ( | float | joint_control_weights[NUM_JOINTS] | ) |
Get how much each joint is controlled by current behavior calculations.
Similar to get_behavior_joint_weights(), but for current behavior.
AtlasErrorCode AtlasSimInterface::get_estimated_position | ( | AtlasPositionData & | robot_pos_est, |
AtlasVec3f | foot_pos_est[Atlas::NUM_FEET] | ||
) |
Accessor to Atlas internal position estimates.
This function will return Atlas internal odometry estimates without having to call process_control_input().
[out] | robot_pos_est | - internal odometry position estimate of robot base (pelvis) |
[out] | foot_pos_est | - internal odometry position estimate of feet |
Position estimates are with respect to an integrated odometry frame which starts at (0, 0). Robot base orientation is assumed to be consistent with current IMU orientation.
Results may be invalid if control hasn't been run yet.
std::string AtlasSimInterface::get_error_code_text | ( | AtlasErrorCode | ec | ) |
Get string version of error code.
AtlasSimInterface* create_atlas_sim_interface | ( | ) | [friend] |
Create a simulation interface object.
This function creates an AtlasSimInterface object. It should not be called more than once. Multiple calls will return the same object.
The object should not be destroyed using delete, but instead by calling destroy_atlas_sim_interface().
void destroy_atlas_sim_interface | ( | ) | [friend] |
Destroy the simulation interface object created by create_atlas_sim_interface().