humotion / src / server / reflexxes_motion_generator.cpp @ 1efa78e9
History | View | Annotate | Download (3.918 KB)
1 | 8c6c1163 | Simon Schulz | /*
|
---|---|---|---|
2 | * This file is part of humotion
|
||
3 | *
|
||
4 | * Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
|
||
5 | * http://opensource.cit-ec.de/projects/humotion
|
||
6 | *
|
||
7 | * This file may be licensed under the terms of the
|
||
8 | * GNU Lesser General Public License Version 3 (the ``LGPL''),
|
||
9 | * or (at your option) any later version.
|
||
10 | *
|
||
11 | * Software distributed under the License is distributed
|
||
12 | * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
|
||
13 | * express or implied. See the LGPL for the specific language
|
||
14 | * governing rights and limitations.
|
||
15 | *
|
||
16 | * You should have received a copy of the LGPL along with this
|
||
17 | * program. If not, go to http://www.gnu.org/licenses/lgpl.html
|
||
18 | * or write to the Free Software Foundation, Inc.,
|
||
19 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
||
20 | *
|
||
21 | * The development of this software was supported by the
|
||
22 | * Excellence Cluster EXC 277 Cognitive Interaction Technology.
|
||
23 | * The Excellence Cluster EXC 277 is a grant of the Deutsche
|
||
24 | * Forschungsgemeinschaft (DFG) in the context of the German
|
||
25 | * Excellence Initiative.
|
||
26 | */
|
||
27 | |||
28 | #include "server/eye_motion_generator.h" |
||
29 | |||
30 | using namespace std; |
||
31 | using namespace humotion; |
||
32 | using namespace humotion::server; |
||
33 | |||
34 | //! constructor
|
||
35 | ReflexxesMotionGenerator::ReflexxesMotionGenerator(JointInterface *j, int dof, float t) : MotionGenerator(j){ |
||
36 | dof_count = dof; |
||
37 | |||
38 | 21444915 | Simon Schulz | //create Reflexxes API for <dof> DOF actuator
|
39 | 8c6c1163 | Simon Schulz | reflexxes_api = new ReflexxesAPI(dof, t);
|
40 | reflexxes_position_input = new RMLPositionInputParameters(dof);
|
||
41 | reflexxes_position_output = new RMLPositionOutputParameters(dof);
|
||
42 | |||
43 | //synchronize phase
|
||
44 | reflexxes_motion_flags.SynchronizationBehavior = RMLPositionFlags::PHASE_SYNCHRONIZATION_IF_POSSIBLE; |
||
45 | } |
||
46 | |||
47 | //! destructor
|
||
48 | ReflexxesMotionGenerator::~ReflexxesMotionGenerator(){ |
||
49 | |||
50 | } |
||
51 | |||
52 | //! feed motion generator with target data:
|
||
53 | //! \param dof id
|
||
54 | //! \param target angle
|
||
55 | //! \param max_speed max reachable speed during accel
|
||
56 | //! \param max_accel max allowable acceleration
|
||
57 | 0d0f5ca1 | Simon Schulz | void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target, float current_position, float current_speed, float max_speed, float max_accel){ |
58 | 21444915 | Simon Schulz | assert(dof < dof_count); |
59 | |||
60 | 8c6c1163 | Simon Schulz | //set up reflexxes:
|
61 | reflexxes_position_input->TargetPositionVector->VecData[dof] = target; |
||
62 | reflexxes_position_input->SelectionVector->VecData[dof] = true;
|
||
63 | reflexxes_position_input->MaxVelocityVector->VecData[dof] = max_speed; |
||
64 | 21444915 | Simon Schulz | reflexxes_position_input->MaxAccelerationVector->VecData[dof] = max_accel; |
65 | 8c6c1163 | Simon Schulz | reflexxes_position_input->TargetVelocityVector->VecData[dof] = 0.0; //target speed is zero (really?) |
66 | 0d0f5ca1 | Simon Schulz | // feed back current pos & velocity
|
67 | 1efa78e9 | sschulz | //reflexxes_position_input->CurrentPositionVector->VecData[dof] = current_position;
|
68 | //reflexxes_position_input->CurrentVelocityVector->VecData[dof] = current_speed;
|
||
69 | 21444915 | Simon Schulz | |
70 | // safety: libreflexxes does not like zero accellerations...
|
||
71 | if (reflexxes_position_input->MaxAccelerationVector->VecData[dof] == 0.0){ |
||
72 | reflexxes_position_input->MaxAccelerationVector->VecData[dof] = 0.0001; |
||
73 | } |
||
74 | 8c6c1163 | Simon Schulz | } |
75 | |||
76 | 0d0f5ca1 | Simon Schulz | |
77 | 8c6c1163 | Simon Schulz | //! calculate motion profile
|
78 | |||
79 | void ReflexxesMotionGenerator::reflexxes_calculate_profile(){
|
||
80 | int res = reflexxes_api->RMLPosition(*reflexxes_position_input, reflexxes_position_output, reflexxes_motion_flags);
|
||
81 | |||
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);
|
||
87 | } |
||
88 | } |
||
89 | |||
90 | //feed back values:
|
||
91 | for(int i=0; i<dof_count; i++){ |
||
92 | 1efa78e9 | sschulz | 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]; |
||
95 | 8c6c1163 | Simon Schulz | } |
96 | } |