humotion / src / server / motion_generator.cpp @ 35b3ca25
History | View | Annotate | Download (5.031 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/motion_generator.h" |
||
29 | cc0dd967 | Simon Schulz | #include "boost/date_time/posix_time/posix_time.hpp" |
30 | 8c6c1163 | Simon Schulz | |
31 | using namespace std; |
||
32 | using namespace humotion; |
||
33 | using namespace humotion::server; |
||
34 | using namespace boost; |
||
35 | |||
36 | //! constructor
|
||
37 | MotionGenerator::MotionGenerator(JointInterface *j){ |
||
38 | joint_interface = j; |
||
39 | cc0dd967 | Simon Schulz | last_mouth_target_update = posix_time::ptime(posix_time::min_date_time); |
40 | last_gaze_target_update = posix_time::ptime(posix_time::min_date_time); |
||
41 | 8c6c1163 | Simon Schulz | } |
42 | |||
43 | |||
44 | //! destructor
|
||
45 | MotionGenerator::~MotionGenerator(){ |
||
46 | } |
||
47 | |||
48 | 730467d3 | Simon Schulz | //! fetch the latest position and velocity and return the timestamp of that dataset
|
49 | //! \param joint id
|
||
50 | //! \param pointer to position variable
|
||
51 | //! \param pointer to veolocity variable
|
||
52 | //! \return Timestamp of this dataset
|
||
53 | humotion::Timestamp MotionGenerator::get_timestamped_state(int joint_id,
|
||
54 | float *position, float *velocity) { |
||
55 | humotion::Timestamp stamp = joint_interface->get_ts_position(joint_id).get_last_timestamp(); |
||
56 | *position = joint_interface->get_ts_position(joint_id).get_interpolated_value(stamp); |
||
57 | *velocity = joint_interface->get_ts_speed(joint_id).get_interpolated_value(stamp); |
||
58 | return stamp;
|
||
59 | } |
||
60 | |||
61 | 0d0f5ca1 | Simon Schulz | //! fetch the latest (=current) speed of a joint
|
62 | //! \param joint_id
|
||
63 | //! \return float value of joint speed
|
||
64 | float MotionGenerator::get_current_speed(int joint_id){ |
||
65 | return joint_interface->get_ts_speed(joint_id).get_newest_value();
|
||
66 | } |
||
67 | |||
68 | //! fetch the latest (=current) position of a joint
|
||
69 | 8c6c1163 | Simon Schulz | //! \param joint_id
|
70 | //! \return float value of joint position
|
||
71 | float MotionGenerator::get_current_position(int joint_id){ |
||
72 | 21444915 | Simon Schulz | /*Timestamp tsl = joint_interface->get_ts_position(joint_id).get_last_timestamp();
|
73 | Timestamp now = Timestamp::now();
|
||
74 | Timestamp diff=now-tsl;
|
||
75 | printf("TIME DIFF %fs %fns\n",diff.sec, diff.nsec);*/
|
||
76 | |||
77 | 8c6c1163 | Simon Schulz | return joint_interface->get_ts_position(joint_id).get_newest_value();
|
78 | } |
||
79 | |||
80 | //! update gaze target:
|
||
81 | //! \param GazeState with target values for the overall gaze
|
||
82 | void MotionGenerator::set_gaze_target(GazeState new_gaze_target){
|
||
83 | //store value for next iteration
|
||
84 | requested_gaze_state = new_gaze_target; |
||
85 | |||
86 | //keep track if the gaze targets are comming in regulary
|
||
87 | last_gaze_target_update = get_system_time(); |
||
88 | } |
||
89 | |||
90 | //! update mouth state:
|
||
91 | //! \param MouthState with target values for the mouth joints
|
||
92 | void MotionGenerator::set_mouth_target(MouthState s){
|
||
93 | //store value
|
||
94 | requested_mouth_target = s; |
||
95 | |||
96 | //keep track if the mouth targets are comming in regulary
|
||
97 | last_mouth_target_update = get_system_time(); |
||
98 | } |
||
99 | |||
100 | |||
101 | //! was there incoming gaze data the last second?
|
||
102 | //! \return true if there was data incoming in the last second, false otherwise
|
||
103 | bool MotionGenerator::gaze_target_input_active(){
|
||
104 | 0d2003bb | Simon Schulz | if (last_gaze_target_update + posix_time::milliseconds(1000) > get_system_time() ){ |
105 | 8c6c1163 | Simon Schulz | //incoming data -> if gaze is disabled, enable it!
|
106 | joint_interface->enable_gaze_joints(); |
||
107 | return true; |
||
108 | }else{
|
||
109 | //no incoming data, disable!
|
||
110 | joint_interface->disable_gaze_joints(); |
||
111 | return false; |
||
112 | } |
||
113 | } |
||
114 | |||
115 | //! was there incoming mouth data the last second?
|
||
116 | //! \return true if there was data incoming in the last second, false otherwise
|
||
117 | bool MotionGenerator::mouth_target_input_active(){
|
||
118 | if (last_mouth_target_update + posix_time::milliseconds(1000) > get_system_time() ){ |
||
119 | //incoming data -> if mouth is disabled, enable it!
|
||
120 | joint_interface->enable_mouth_joints(); |
||
121 | return true; |
||
122 | }else{
|
||
123 | //no incoming data, disable!
|
||
124 | joint_interface->disable_mouth_joints(); |
||
125 | return false; |
||
126 | } |
||
127 | } |
||
128 | |||
129 | //! limit target to min/max bounds:
|
||
130 | float MotionGenerator::limit_target(int joint_id, float val){ |
||
131 | assert(joint_id < JointInterface::JOINT_ID_ENUM_SIZE); |
||
132 | |||
133 | //fetch min/max for joint:
|
||
134 | float min = joint_interface->get_joint_min(joint_id);
|
||
135 | float max = joint_interface->get_joint_max(joint_id);
|
||
136 | |||
137 | if (max < min){
|
||
138 | printf("> ERROR: how can min (%4.2f) be bigger than max (%4.2f)?? FIX THIS! EXITING NOW\n",min,max);
|
||
139 | exit(0);
|
||
140 | } |
||
141 | |||
142 | val = fmin(val, max); |
||
143 | val = fmax(val, min); |
||
144 | |||
145 | return val;
|
||
146 | } |