Revision 0c8d22a5 src/server/mouth_motion_generator.cpp
| src/server/mouth_motion_generator.cpp | ||
|---|---|---|
| 25 | 25 |
* Excellence Initiative. |
| 26 | 26 |
*/ |
| 27 | 27 |
|
| 28 |
#include "server/mouth_motion_generator.h" |
|
| 28 |
#include "humotion/server/mouth_motion_generator.h"
|
|
| 29 | 29 |
|
| 30 |
using namespace std; |
|
| 31 |
using namespace humotion; |
|
| 32 |
using namespace humotion::server; |
|
| 30 |
using humotion::server::MouthMotionGenerator; |
|
| 33 | 31 |
|
| 34 |
//minimum mouth opening:
|
|
| 35 |
const float MouthMotionGenerator::MOUTH_MIN_OPENING = 9.0; //mm
|
|
| 32 |
// minimum mouth opening
|
|
| 33 |
const float MouthMotionGenerator::MOUTH_MIN_OPENING = 9.0; // mm
|
|
| 36 | 34 |
|
| 37 | 35 |
//! constructor |
| 38 |
MouthMotionGenerator::MouthMotionGenerator(JointInterface *j) : MotionGenerator(j){
|
|
| 36 |
MouthMotionGenerator::MouthMotionGenerator(JointInterface *j) : MotionGenerator(j) {
|
|
| 39 | 37 |
} |
| 40 | 38 |
|
| 41 | 39 |
//! destructor |
| 42 |
MouthMotionGenerator::~MouthMotionGenerator(){
|
|
| 43 |
|
|
| 40 |
MouthMotionGenerator::~MouthMotionGenerator() {
|
|
| 44 | 41 |
} |
| 45 | 42 |
|
| 46 | 43 |
//! calculate joint targets |
| 47 |
void MouthMotionGenerator::calculate_targets(){
|
|
| 48 |
///printf("> humotion: calculating mouth targets\n");
|
|
| 44 |
void MouthMotionGenerator::calculate_targets() {
|
|
| 45 |
// printf("> humotion: calculating mouth targets\n");
|
|
| 49 | 46 |
update_mouth_target(JointInterface::ID_LIP_LEFT_UPPER, JointInterface::ID_LIP_LEFT_LOWER); |
| 50 | 47 |
update_mouth_target(JointInterface::ID_LIP_CENTER_UPPER, JointInterface::ID_LIP_CENTER_LOWER); |
| 51 | 48 |
update_mouth_target(JointInterface::ID_LIP_RIGHT_UPPER, JointInterface::ID_LIP_RIGHT_LOWER); |
| ... | ... | |
| 54 | 51 |
//! calculate mouth target angles for a given combination of upper/lower joints |
| 55 | 52 |
//! \param int upper joint id |
| 56 | 53 |
//! \param int lower joint id |
| 57 |
void MouthMotionGenerator::update_mouth_target(int upper_id, int lower_id){
|
|
| 58 |
//fetch min/max for joints:
|
|
| 54 |
void MouthMotionGenerator::update_mouth_target(int upper_id, int lower_id) {
|
|
| 55 |
// fetch min/max for joints
|
|
| 59 | 56 |
float min_upper = joint_interface->get_joint_min(upper_id); |
| 60 | 57 |
float max_lower = joint_interface->get_joint_max(lower_id); |
| 61 | 58 |
float min_opening = MOUTH_MIN_OPENING; |
| 62 | 59 |
|
| 63 |
//fetch position & opening for joint, parameter is only used to determine LEFT/CENTER/RIGHT. upper/lower plays no role here: |
|
| 60 |
// fetch position & opening for joint, parameter is only used to |
|
| 61 |
// determine LEFT/CENTER/RIGHT. upper/lower plays no role here |
|
| 64 | 62 |
float position = mouthstate_to_position(requested_mouth_target, upper_id); |
| 65 | 63 |
float opening = min_opening + mouthstate_to_opening(requested_mouth_target, upper_id); |
| 66 | 64 |
|
| 67 |
//check opening larger than minimum:
|
|
| 68 |
if (opening < min_opening){
|
|
| 69 |
//oops, not safe to move to this opening, abort move! |
|
| 70 |
//printf("> invalid opening (%f)\n", opening);
|
|
| 65 |
// check opening larger than minimum
|
|
| 66 |
if (opening < min_opening) {
|
|
| 67 |
// oops, not safe to move to this opening, abort move!
|
|
| 68 |
// printf("> invalid opening (%f)\n", opening);
|
|
| 71 | 69 |
opening = min_opening; |
| 72 | 70 |
} |
| 73 | 71 |
|
| 74 | 72 |
float unsafe_target_upper = position - opening/2.0; |
| 75 | 73 |
float unsafe_target_lower = position + opening/2.0; |
| 76 | 74 |
|
| 77 |
//now check if this would exceed the allowed limits for that joint:
|
|
| 75 |
// now check if this would exceed the allowed limits for that joint
|
|
| 78 | 76 |
float distance_min_upper = unsafe_target_upper - min_upper; |
| 79 |
if (distance_min_upper < 0){
|
|
| 80 |
//oops, we will get to close to the safe border, abort move! |
|
| 81 |
//printf("> collision on TOP [u=%4.2f l=%4.2f]\n",unsafe_target_upper, unsafe_target_lower);
|
|
| 77 |
if (distance_min_upper < 0) {
|
|
| 78 |
// oops, we will get to close to the safe border, abort move!
|
|
| 79 |
// printf("> collision on TOP [u=%4.2f l=%4.2f]\n",unsafe_target_upper,unsafe_target_lower);
|
|
| 82 | 80 |
unsafe_target_upper = min_upper; |
| 83 | 81 |
unsafe_target_lower = unsafe_target_upper + opening; |
| 84 | 82 |
} |
| 85 | 83 |
|
| 86 | 84 |
float distance_max_lower = max_lower - unsafe_target_lower; |
| 87 |
if (distance_max_lower < 0){
|
|
| 88 |
//oops, we will get to close to the safe border, abort move! |
|
| 89 |
//printf("> collision on BOT[u=%4.2f l=%4.2f]\n",unsafe_target_upper, unsafe_target_lower);
|
|
| 85 |
if (distance_max_lower < 0) {
|
|
| 86 |
// oops, we will get to close to the safe border, abort move!
|
|
| 87 |
// printf("> collision on BOT[u=%4.2f l=%4.2f]\n",unsafe_target_upper,unsafe_target_lower);
|
|
| 90 | 88 |
unsafe_target_lower = max_lower; |
| 91 | 89 |
unsafe_target_upper = unsafe_target_lower - opening; |
| 92 | 90 |
} |
| 93 | 91 |
|
| 94 |
// tell the joint about the new values:
|
|
| 92 |
// tell the joint about the new values |
|
| 95 | 93 |
joint_interface->set_target(upper_id, unsafe_target_upper, 0.0); |
| 96 | 94 |
joint_interface->set_target(lower_id, unsafe_target_lower, 0.0); |
| 97 | 95 |
} |
| ... | ... | |
| 100 | 98 |
//! extract opening from mouth state for given joint enum id |
| 101 | 99 |
//! \param MouthState m |
| 102 | 100 |
//! \param int enum with joint id |
| 103 |
float MouthMotionGenerator::mouthstate_to_opening(MouthState m, int e){
|
|
| 104 |
switch (e){
|
|
| 105 |
default: printf("> get_opening(0x%02X): invalid joint enum!\n",e); exit(0);
|
|
| 101 |
float MouthMotionGenerator::mouthstate_to_opening(MouthState m, int e) {
|
|
| 102 |
switch (e) {
|
|
| 103 |
default: printf("> get_opening(0x%02X): invalid joint enum!\n", e); exit(EXIT_FAILURE);
|
|
| 106 | 104 |
case(JointInterface::ID_LIP_LEFT_UPPER): |
| 107 | 105 |
case(JointInterface::ID_LIP_LEFT_LOWER): |
| 108 | 106 |
return m.opening_left; |
| ... | ... | |
| 118 | 116 |
//! extract position from mouth state for given joint enum id |
| 119 | 117 |
//! \param MouthState m |
| 120 | 118 |
//! \param int enum with joint id |
| 121 |
float MouthMotionGenerator::mouthstate_to_position(MouthState m, int e){
|
|
| 122 |
switch (e){
|
|
| 123 |
default: printf("> get_position(0x%02X): invalid joint enum!\n",e); exit(0);
|
|
| 119 |
float MouthMotionGenerator::mouthstate_to_position(MouthState m, int e) {
|
|
| 120 |
switch (e) {
|
|
| 121 |
default: printf("> get_position(0x%02X): invalid joint enum!\n", e); exit(EXIT_FAILURE);
|
|
| 124 | 122 |
case(JointInterface::ID_LIP_LEFT_UPPER): |
| 125 | 123 |
case(JointInterface::ID_LIP_LEFT_LOWER): |
| 126 | 124 |
return m.position_left; |
| ... | ... | |
| 134 | 132 |
} |
| 135 | 133 |
|
| 136 | 134 |
//! publish targets to motor boards: |
| 137 |
void MouthMotionGenerator::publish_targets(){
|
|
| 138 |
//publish values if there is an active gaze input within the last timerange |
|
| 139 |
if (mouth_target_input_active()){
|
|
| 135 |
void MouthMotionGenerator::publish_targets() {
|
|
| 136 |
// publish values if there is an active gaze input within the last timerange
|
|
| 137 |
if (mouth_target_input_active()) {
|
|
| 140 | 138 |
joint_interface->publish_target(JointInterface::ID_LIP_LEFT_UPPER); |
| 141 | 139 |
joint_interface->publish_target(JointInterface::ID_LIP_LEFT_LOWER); |
| 142 | 140 |
joint_interface->publish_target(JointInterface::ID_LIP_CENTER_UPPER); |
Also available in: Unified diff