Revision 0c8d22a5 src/server/reflexxes_motion_generator.cpp
| src/server/reflexxes_motion_generator.cpp | ||
|---|---|---|
| 25 | 25 |
* Excellence Initiative. |
| 26 | 26 |
*/ |
| 27 | 27 |
|
| 28 |
#include "server/eye_motion_generator.h" |
|
| 28 |
#include "humotion/server/eye_motion_generator.h"
|
|
| 29 | 29 |
|
| 30 |
using namespace std; |
|
| 31 |
using namespace humotion; |
|
| 32 |
using namespace humotion::server; |
|
| 30 |
using humotion::server::ReflexxesMotionGenerator; |
|
| 33 | 31 |
|
| 34 | 32 |
//! constructor |
| 35 |
ReflexxesMotionGenerator::ReflexxesMotionGenerator(JointInterface *j, int dof, float t) : MotionGenerator(j){
|
|
| 33 |
ReflexxesMotionGenerator::ReflexxesMotionGenerator(JointInterface *j, int dof, float t) : |
|
| 34 |
MotionGenerator(j) {
|
|
| 36 | 35 |
dof_count = dof; |
| 37 | 36 |
|
| 38 |
//create Reflexxes API for <dof> DOF actuator |
|
| 37 |
// create Reflexxes API for <dof> DOF actuator
|
|
| 39 | 38 |
reflexxes_api = new ReflexxesAPI(dof, t); |
| 40 | 39 |
reflexxes_position_input = new RMLPositionInputParameters(dof); |
| 41 | 40 |
reflexxes_position_output = new RMLPositionOutputParameters(dof); |
| 42 | 41 |
|
| 43 |
//synchronize phase |
|
| 44 |
reflexxes_motion_flags.SynchronizationBehavior = RMLPositionFlags::PHASE_SYNCHRONIZATION_IF_POSSIBLE; |
|
| 42 |
// synchronize phase |
|
| 43 |
reflexxes_motion_flags.SynchronizationBehavior = |
|
| 44 |
RMLPositionFlags::PHASE_SYNCHRONIZATION_IF_POSSIBLE; |
|
| 45 | 45 |
} |
| 46 | 46 |
|
| 47 | 47 |
//! destructor |
| 48 |
ReflexxesMotionGenerator::~ReflexxesMotionGenerator(){
|
|
| 49 |
|
|
| 48 |
ReflexxesMotionGenerator::~ReflexxesMotionGenerator() {
|
|
| 50 | 49 |
} |
| 51 | 50 |
|
| 52 | 51 |
//! feed motion generator with target data: |
| ... | ... | |
| 55 | 54 |
//! \param max_speed max reachable speed during accel |
| 56 | 55 |
//! \param max_accel max allowable acceleration |
| 57 | 56 |
void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target, |
| 58 |
float current_position, float current_speed,
|
|
| 57 |
float current_position, float current_velocity,
|
|
| 59 | 58 |
humotion::Timestamp timestamp, |
| 60 |
float max_speed, float max_accel){
|
|
| 59 |
float max_speed, float max_accel) {
|
|
| 61 | 60 |
assert(dof < dof_count); |
| 62 | 61 |
|
| 63 | 62 |
// set up reflexxes control loop |
| ... | ... | |
| 65 | 64 |
reflexxes_position_input->SelectionVector->VecData[dof] = true; |
| 66 | 65 |
reflexxes_position_input->MaxVelocityVector->VecData[dof] = max_speed; |
| 67 | 66 |
reflexxes_position_input->MaxAccelerationVector->VecData[dof] = max_accel; |
| 68 |
reflexxes_position_input->TargetVelocityVector->VecData[dof] = 0.0; //target speed is zero (really?) |
|
| 67 |
|
|
| 68 |
// target speed is zero (really?) |
|
| 69 |
reflexxes_position_input->TargetVelocityVector->VecData[dof] = 0.0; |
|
| 69 | 70 |
|
| 70 | 71 |
// safety: libreflexxes does not like zero accellerations... |
| 71 |
if (reflexxes_position_input->MaxAccelerationVector->VecData[dof] == 0.0){
|
|
| 72 |
if (reflexxes_position_input->MaxAccelerationVector->VecData[dof] == 0.0) {
|
|
| 72 | 73 |
reflexxes_position_input->MaxAccelerationVector->VecData[dof] = 0.0001; |
| 73 | 74 |
} |
| 74 | 75 |
} |
| ... | ... | |
| 76 | 77 |
|
| 77 | 78 |
//! calculate motion profile |
| 78 | 79 |
|
| 79 |
void ReflexxesMotionGenerator::reflexxes_calculate_profile(){
|
|
| 80 |
int res = reflexxes_api->RMLPosition(*reflexxes_position_input, reflexxes_position_output, reflexxes_motion_flags); |
|
| 80 |
void ReflexxesMotionGenerator::reflexxes_calculate_profile() {
|
|
| 81 |
int res = reflexxes_api->RMLPosition(*reflexxes_position_input, |
|
| 82 |
reflexxes_position_output, reflexxes_motion_flags); |
|
| 81 | 83 |
|
| 82 |
if (res < 0){
|
|
| 83 |
if (res == ReflexxesAPI::RML_ERROR_INVALID_INPUT_VALUES){
|
|
| 84 |
printf("> ReflexxesMotionGenerator --> ReflexxesAPI::RML_ERROR_INVALID_INPUT_VALUES error\n");
|
|
| 85 |
}else{
|
|
| 86 |
printf("> ReflexxesMotionGenerator --> ReflexxesAPI::UNKNOWN_ERROR: reflexxes error %d\n",res);
|
|
| 84 |
if (res < 0) {
|
|
| 85 |
if (res == ReflexxesAPI::RML_ERROR_INVALID_INPUT_VALUES) {
|
|
| 86 |
printf("> ReflexxesMotionGenerator --> RML_ERROR_INVALID_INPUT_VALUES error\n");
|
|
| 87 |
} else {
|
|
| 88 |
printf("> ReflexxesMotionGenerator --> UNKNOWN_ERROR: reflexxes error %d\n", res);
|
|
| 87 | 89 |
} |
| 88 | 90 |
} |
| 89 | 91 |
|
| 90 | 92 |
// feed back values |
| 91 |
for(int i=0; i<dof_count; i++){
|
|
| 92 |
reflexxes_position_input->CurrentPositionVector->VecData[i] = reflexxes_position_output->NewPositionVector->VecData[i]; |
|
| 93 |
reflexxes_position_input->CurrentVelocityVector->VecData[i] = reflexxes_position_output->NewVelocityVector->VecData[i]; |
|
| 94 |
reflexxes_position_input->CurrentAccelerationVector->VecData[i] = reflexxes_position_output->NewAccelerationVector->VecData[i]; |
|
| 93 |
for (int i = 0; i < dof_count; i++) {
|
|
| 94 |
reflexxes_position_input->CurrentPositionVector->VecData[i] = |
|
| 95 |
reflexxes_position_output->NewPositionVector->VecData[i]; |
|
| 96 |
|
|
| 97 |
reflexxes_position_input->CurrentVelocityVector->VecData[i] = |
|
| 98 |
reflexxes_position_output->NewVelocityVector->VecData[i]; |
|
| 99 |
|
|
| 100 |
reflexxes_position_input->CurrentAccelerationVector->VecData[i] = |
|
| 101 |
reflexxes_position_output->NewAccelerationVector->VecData[i]; |
|
| 95 | 102 |
} |
| 96 | 103 |
} |
Also available in: Unified diff