|
SMORES Robot Platform Simulation
Modlab at Penn, ASL at Cornell
|
Public Member Functions | |
| virtual void | ExtraInitializationInLoad (physics::WorldPtr _parent, sdf::ElementPtr _sdf) |
| This function will be called in Load() to perform extra initialization. | |
| LibraryTemplate * | DynamicallyLoadedLibrary (const char *library_path, void *lib_handle) |
| Load a shared library in linux. | |
| void | CloseLoadedLibrary (void **lib_handle) |
| Close the loaded libraries. | |
| virtual void | OnSystemRunningExtra (const common::UpdateInfo &_info) |
| virtual void | InsertModel (string name, math::Pose position, string joint_angles) |
| Insert a model to the current world. More... | |
| virtual void | InsertModel (string name, math::Pose position, string joint_angles, string model_path) |
| Insert a model to the current world. More... | |
| void | AddInitialPosition (math::Pose position) |
| Add a new position to set at the end of 'intialPosition' vector. | |
| void | AddInitialJoints (string joint_angles) |
| Add new initial joint values to set at the end of 'initalJointValue' vector. | |
| void | DeleteSmoresmodulePtr (string module_name) |
| Delete the smoresmodule object in the world. More... | |
| void | DeleteModule (string module_name) |
| Delete a model that already in the world. More... | |
| void | DeleteAllModules (void) |
| Delete all models that already in the world. | |
| math::Vector3 | GetCurrentConfigurationPose (void) |
| Get the current position in world of the inserted configuration. More... | |
| void | BuildConfigurationFromXML (string file_name) |
| Build a configuration at origin using a XML file. | |
| void | BuildConfigurationFromXML (string file_name, math::Vector3 initial_pose) |
| Build a configuration at given initial_pose using a XML file. | |
| void | BuildConnectionFromXML (string file_name) |
| This function is used to build connection using a XML file. | |
| virtual void | ExtraWorkWhenModelInserted (CommandMessagePtr &msg) |
| This function will be called after set the model initial position. | |
| double | ConversionForAngleOverPi (double angle) |
| Convert angles so that their absolutely value always smaller than Pi. | |
| void | EnableAutoMagneticConnection (void) |
| void | PassiveConnect (SmoresModulePtr module_1, SmoresModulePtr module_2, int node1_ID, int node2_ID, double node_angle, double node_distance) |
| Connect two modules by pointers and node_ID. | |
| void | PassiveConnect (SmoresModulePtr module_1, SmoresModulePtr module_2, int node1_ID, int node2_ID) |
| With default angle and distance offset equal to 0. | |
| void | ActiveConnect (SmoresModulePtr module_1, SmoresModulePtr module_2, int node1_ID, int node2_ID, double node_angle, double node_distance) |
| TODO: Need add 'active' feature to this function. | |
| void | ActiveConnect (SmoresModulePtr module_1, SmoresModulePtr module_2, int node1_ID, int node2_ID) |
| With default angle and distance offset equal to 0. | |
| void | Disconnect (SmoresEdgePtr aEdge) |
| void | Disconnect (SmoresModulePtr aModule, int node_ID) |
| Disconnect two module base on one module and one node of that module. | |
| void | Disconnect (string moduleName, int node_ID) |
| Disconnect two module base on one module and one node of that module. | |
| void | Disconnect (string moduleName1, string moduleName2) |
| Disconnect two module base on their names. | |
| void | SendGaitTable (SmoresModulePtr module, const int *flag, const double *gait_value, int msg_type, unsigned int time_stamp, string condition_str, string dependency_str) |
| Send time based common gait with condition and dependency. More... | |
| void | SendGaitTable (SmoresModulePtr module, const int *flag, const double *gait_value, int msg_type, unsigned int time_stamp) |
| Send time based common gait without condition and dependency. More... | |
| void | SendGaitTable (SmoresModulePtr module, const int *flag, const double *gait_value, int msg_type, string condition_str, string dependency_str) |
| Send common gait with condition and dependency. More... | |
| void | SendGaitTable (SmoresModulePtr module, const int *flag, const double *gait_value, int msg_type) |
| Send common gait without condition and dependency. More... | |
| void | SendGaitTable (SmoresModulePtr module, int joint_ID, double gait_value, int msg_type, unsigned int time_stamp, string condition_str, string dependency_str) |
| Send gait table to change a single joint of a module. More... | |
| void | SendGaitTable (SmoresModulePtr module, int joint_ID, double gait_value, int msg_type, unsigned int time_stamp) |
| Send gait table to change a single joint of a module. More... | |
| void | SendGaitTable (SmoresModulePtr module, int joint_ID, double gait_value, int msg_type, string condition_str, string dependency_str) |
| Send gait table to change a single joint of a module. More... | |
| void | SendGaitTable (SmoresModulePtr module, int joint_ID, double gait_value, int msg_type) |
| Send gait table to change a single joint of a module. More... | |
| void | SendGaitTable (SmoresModulePtr module, string module1, string module2, int node1, int node2, int commandtype, unsigned int time_stamp, string condition_str, string dependency_str) |
| void | SendGaitTable (SmoresModulePtr module, string module1, string module2, int node1, int node2, int commandtype, unsigned int time_stamp) |
| void | SendGaitTable (SmoresModulePtr module, string module1, string module2, int node1, int node2, int commandtype, string condition_str, string dependency_str) |
| void | SendGaitTable (SmoresModulePtr module, string module1, string module2, int node1, int node2, int commandtype) |
| void | SendGaitTableInstance (SmoresModulePtr module, const int *flag, const double *gait_value, int msg_type) |
| Used in the direct driving situation, ignore execution order. More... | |
| void | SendGaitTableInstance (SmoresModulePtr module, const int *flag, const double *gait_value) |
| Used in the direct driving situation with default type 4. More... | |
| void | SendPositionInstance (SmoresModulePtr module, double x, double y, double orientation_angle) |
| Used in direct control, dirve a module to a specific position. More... | |
| void | EraseComaands (SmoresModulePtr module) |
| void | DynamicJointDestroy (SmoresEdgePtr aEdge) |
| void | ReBuildDynamicJoint (SmoresEdgePtr a_edge) |
| Rebuild the dynamic joint using the information in the edge object. | |
| int | GetNodeIDByName (string node_name) |
| bool | CheckModuleExistByName (string module_name) |
| Get return true if the smores module with given name exist in the world. | |
| SmoresModulePtr | GetModulePtrByName (string module_name) |
| Get SmoresModule object by specifying the name. More... | |
| SmoresModulePtr | GetModulePtrByIDX (unsigned int idx) |
| Get SmoresModule object by specifying the index in the vector moduleList. | |
| unsigned int | GetModuleListSize (void) |
| Get the count of the modules that are in the list. | |
| void | EraseCommandPtrByModule (SmoresModulePtr module_ptr) |
| int | GetModuleIndexByName (string module_name) |
| bool | AlreadyConnected (SmoresModulePtr module_1, SmoresModulePtr module_2, int node1_ID, int node2_ID) |
| Check whether two nodes are connected together. | |
| bool | AlreadyConnected (SmoresModulePtr module_1, SmoresModulePtr module_2) |
| Check whether two modules have already connected on a node. | |
| bool | AlreadyConnected (SmoresModulePtr module, int node_ID) |
| Check whether a node a of module has been occupied. | |
| void | ReadFileAndGenerateCommands (const char *fileName) |
| Read a 'gait table' stored in a text file. | |
| void | FigureInterpret (const vector< string > *joints_spec, int *type_flags, double *joint_values) |
| Used to interpret the number in gait table. More... | |
| void | InterpretCommonGaitString (string a_command_str) |
| Interpret normal command string. More... | |
| void | InterpretSpecialString (string a_command_str) |
| Interpret special command. More... | |
| unsigned int | CountModules (SmoresModulePtr module) |
| Count how many modules are there in a cluster with the current module. More... | |
| unsigned int | GetInitialJointSequenceSize (void) |
| Get the length of the initial joint value setting sequence. | |
| unsigned int | GetEdgeCounts (void) |
| Get the total counts of the edges that stored in the program. | |
| SmoresEdgePtr | GetEdgePtrByIDX (unsigned int idx) |
| Get SmoresEdge object by specifying the index in the vector connectionEdges. | |
Public Attributes | |
| physics::WorldPtr | currentWorld |
| unsigned int gazebo::WorldServer::CountModules | ( | SmoresModulePtr | module | ) |
Count how many modules are there in a cluster with the current module.
| module | A pointer of SmoresModule object |
| void gazebo::WorldServer::DeleteModule | ( | string | module_name | ) |
Delete a model that already in the world.
| module_name | Module name string |
| void gazebo::WorldServer::DeleteSmoresmodulePtr | ( | string | module_name | ) |
Delete the smoresmodule object in the world.
| module_name | Module name string |
| void gazebo::WorldServer::Disconnect | ( | SmoresEdgePtr | aEdge | ) |
Disconnect two modules on one edge TODO: This pointer must point to an element in the vector
| void gazebo::WorldServer::DynamicJointDestroy | ( | SmoresEdgePtr | aEdge | ) |
Destroyer of the connection between different modules, which is dynamic joint here
| void gazebo::WorldServer::EnableAutoMagneticConnection | ( | void | ) |
Enable auto magnetic connection, all module Have to be called in Load() Default: disabled
| void gazebo::WorldServer::EraseComaands | ( | SmoresModulePtr | module | ) |
Erase all the existing commands of a specific module TODO: Need to be tested
| void gazebo::WorldServer::FigureInterpret | ( | const vector< string > * | joints_spec, |
| int * | type_flags, | ||
| double * | joint_values | ||
| ) |
Used to interpret the number in gait table.
| joints_spec | String tokens of the joint specs |
| type_flags | Joint control flag, see SendGaitTable (return) |
| joint_values | Joint control values (return) |
TODO: This is at a very low API level of SGST need to implement all the features in the future
| math::Vector3 gazebo::WorldServer::GetCurrentConfigurationPose | ( | void | ) |
Get the current position in world of the inserted configuration.
Averaging the pose of all modules return
| SmoresModulePtr gazebo::WorldServer::GetModulePtrByName | ( | string | module_name | ) |
Get SmoresModule object by specifying the name.
| module_name | Module name string |
|
virtual |
Insert a model to the current world.
With joint angles specified
Reimplemented in gazebo::ConfigEditor.
|
virtual |
Insert a model to the current world.
With joint angles and model path specified
Reimplemented in gazebo::ConfigEditor.
| void gazebo::WorldServer::InterpretCommonGaitString | ( | string | a_command_str | ) |
Interpret normal command string.
| a_command_str | A command string |
| void gazebo::WorldServer::InterpretSpecialString | ( | string | a_command_str | ) |
Interpret special command.
| a_command_str | A command string |
| void gazebo::WorldServer::SendGaitTable | ( | SmoresModulePtr | module, |
| const int * | flag, | ||
| const double * | gait_value, | ||
| int | msg_type, | ||
| unsigned int | time_stamp, | ||
| string | condition_str, | ||
| string | dependency_str | ||
| ) |
Send time based common gait with condition and dependency.
| module | A pointer of SmoresModule object |
| flag | Message type for each joint angle 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; 4: connect(Not implemented); 5: disconnect(Not implemented) |
| gait_value | Joints values, either be position, speed or torque |
| msg_type | Defined in command_message.proto, 3 for joint control |
| time_stamp | Time based gait table timer |
| condition_str | Condition string |
| dependency_str | Dependency string |
| void gazebo::WorldServer::SendGaitTable | ( | SmoresModulePtr | module, |
| const int * | flag, | ||
| const double * | gait_value, | ||
| int | msg_type, | ||
| unsigned int | time_stamp | ||
| ) |
Send time based common gait without condition and dependency.
| module | A pointer of SmoresModule object |
| flag | Message type for each joint angle 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; 4: connect(Not implemented); 5: disconnect(Not implemented) |
| gait_value | Joints values, either be position, speed or torque |
| msg_type | Defined in command_message.proto, 3 for joint control |
| time_stamp | Time based gait table timer |
| void gazebo::WorldServer::SendGaitTable | ( | SmoresModulePtr | module, |
| const int * | flag, | ||
| const double * | gait_value, | ||
| int | msg_type, | ||
| string | condition_str, | ||
| string | dependency_str | ||
| ) |
Send common gait with condition and dependency.
| module | A pointer of SmoresModule object |
| flag | Message type for each joint angle 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; 4: connect(Not implemented); 5: disconnect(Not implemented) |
| gait_value | Joints values, either be position, speed or torque |
| msg_type | Defined in command_message.proto, 3 for joint control |
| condition_str | Condition string |
| dependency_str | Dependency string |
| void gazebo::WorldServer::SendGaitTable | ( | SmoresModulePtr | module, |
| const int * | flag, | ||
| const double * | gait_value, | ||
| int | msg_type | ||
| ) |
Send common gait without condition and dependency.
| module | A pointer of SmoresModule object |
| flag | Message type for each joint angle 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; 4: connect(Not implemented); 5: disconnect(Not implemented) |
| gait_value | Joints values, either be position, speed or torque |
| msg_type | Defined in command_message.proto, 3 for joint control |
| void gazebo::WorldServer::SendGaitTable | ( | SmoresModulePtr | module, |
| int | joint_ID, | ||
| double | gait_value, | ||
| int | msg_type, | ||
| unsigned int | time_stamp, | ||
| string | condition_str, | ||
| string | dependency_str | ||
| ) |
Send gait table to change a single joint of a module.
| module | A pointer of SmoresModule object |
| joint_ID | Number index of a joint |
| gait_value | Joint value, either be position, speed or torque |
| msg_type | 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; |
| time_stamp | Time based gait table timer |
| condition_str | Condition string |
| dependency_str | Dependency string |
| void gazebo::WorldServer::SendGaitTable | ( | SmoresModulePtr | module, |
| int | joint_ID, | ||
| double | gait_value, | ||
| int | msg_type, | ||
| unsigned int | time_stamp | ||
| ) |
Send gait table to change a single joint of a module.
| module | A pointer of SmoresModule object |
| joint_ID | Number index of a joint |
| gait_value | Joint value, either be position, speed or torque |
| msg_type | 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; |
| time_stamp | Time based gait table timer |
| void gazebo::WorldServer::SendGaitTable | ( | SmoresModulePtr | module, |
| int | joint_ID, | ||
| double | gait_value, | ||
| int | msg_type, | ||
| string | condition_str, | ||
| string | dependency_str | ||
| ) |
Send gait table to change a single joint of a module.
| module | A pointer of SmoresModule object |
| joint_ID | Number index of a joint |
| gait_value | Joint value, either be position, speed or torque |
| msg_type | 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; |
| condition_str | Condition string |
| dependency_str | Dependency string |
| void gazebo::WorldServer::SendGaitTable | ( | SmoresModulePtr | module, |
| int | joint_ID, | ||
| double | gait_value, | ||
| int | msg_type | ||
| ) |
Send gait table to change a single joint of a module.
| module | A pointer of SmoresModule object |
| joint_ID | Number index of a joint |
| gait_value | Joint value, either be position, speed or torque |
| msg_type | 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; |
| void gazebo::WorldServer::SendGaitTable | ( | SmoresModulePtr | module, |
| string | module1, | ||
| string | module2, | ||
| int | node1, | ||
| int | node2, | ||
| int | commandtype, | ||
| unsigned int | time_stamp, | ||
| string | condition_str, | ||
| string | dependency_str | ||
| ) |
Send connect or disconnect command, with condition or dependency and time based
| module | A pointer of SmoresModule object of module 1 |
| module1 | Name string of module 1 |
| module2 | Name string of module 2 |
| node1 | Number index of the joint of module 1 |
| node2 | Number index of the joint of module 2 |
| commandtype | Special command indicator, 1 connect; 2 disconnect |
| time_stamp | Time based gait table timer |
| condition_str | Condition string |
| dependency_str | Dependency string |
| void gazebo::WorldServer::SendGaitTable | ( | SmoresModulePtr | module, |
| string | module1, | ||
| string | module2, | ||
| int | node1, | ||
| int | node2, | ||
| int | commandtype, | ||
| unsigned int | time_stamp | ||
| ) |
Send connect or disconnect command, without condition or dependency and time based
| module | A pointer of SmoresModule object of module 1 |
| module1 | Name string of module 1 |
| module2 | Name string of module 2 |
| node1 | Number index of the joint of module 1 |
| node2 | Number index of the joint of module 2 |
| commandtype | Special command indicator, 1 connect; 2 disconnect |
| time_stamp | Time based gait table timer |
| void gazebo::WorldServer::SendGaitTable | ( | SmoresModulePtr | module, |
| string | module1, | ||
| string | module2, | ||
| int | node1, | ||
| int | node2, | ||
| int | commandtype, | ||
| string | condition_str, | ||
| string | dependency_str | ||
| ) |
Send connect or disconnect command, with condition or dependency
| module | A pointer of SmoresModule object of module 1 |
| module1 | Name string of module 1 |
| module2 | Name string of module 2 |
| node1 | Number index of the joint of module 1 |
| node2 | Number index of the joint of module 2 |
| commandtype | Special command indicator, 1 connect; 2 disconnect |
| condition_str | Condition string |
| dependency_str | Dependency string |
| void gazebo::WorldServer::SendGaitTable | ( | SmoresModulePtr | module, |
| string | module1, | ||
| string | module2, | ||
| int | node1, | ||
| int | node2, | ||
| int | commandtype | ||
| ) |
Send connect or disconnect command, without condition or dependency
| module | A pointer of SmoresModule object of module 1 |
| module1 | Name string of module 1 |
| module2 | Name string of module 2 |
| node1 | Number index of the joint of module 1 |
| node2 | Number index of the joint of module 2 |
| commandtype | Special command indicator, 1 connect; 2 disconnect |
| void gazebo::WorldServer::SendGaitTableInstance | ( | SmoresModulePtr | module, |
| const int * | flag, | ||
| const double * | gait_value, | ||
| int | msg_type | ||
| ) |
Used in the direct driving situation, ignore execution order.
| module | A pointer of SmoresModule object |
| flag | Message type for each joint angle 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; 4: connect(Not implemented); 5: disconnect(Not implemented) |
| gait_value | Joints values, either be position, speed or torque |
| msg_type | Defined in command_message.proto, 4 for direct drive |
| void gazebo::WorldServer::SendGaitTableInstance | ( | SmoresModulePtr | module, |
| const int * | flag, | ||
| const double * | gait_value | ||
| ) |
Used in the direct driving situation with default type 4.
| module | A pointer of SmoresModule object |
| flag | Message type for each joint angle 0: position; 1: speed; 2: torque(Not implemented); 3: ignore; 4: connect(Not implemented); 5: disconnect(Not implemented) |
| gait_value | Joints values, either be position, speed or torque |
| void gazebo::WorldServer::SendPositionInstance | ( | SmoresModulePtr | module, |
| double | x, | ||
| double | y, | ||
| double | orientation_angle | ||
| ) |
Used in direct control, dirve a module to a specific position.
| module | A pointer of SmoresModule object |
| x | 2D coordinate x |
| y | 2D coordinate y |
| orientation_angle | Final orientation of the module in global frame |