Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Normalize quaternions #40

Merged
merged 1 commit into from
Jan 5, 2018
Merged

Normalize quaternions #40

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 20 additions & 0 deletions interactive_marker_tutorials/scripts/basic_controls.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,14 @@ def saveMarker( int_marker ):
#####################################################################
# Marker Creation

def normalizeQuaternion( quaternion_msg ):
norm = quaternion_msg.x**2 + quaternion_msg.y**2 + quaternion_msg.z**2 + quaternion_msg.w**2
s = norm**(-0.5)
quaternion_msg.x *= s
quaternion_msg.y *= s
quaternion_msg.z *= s
quaternion_msg.w *= s
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As mentioned in the other thread, we should consider providing a function to do this for users.

Ideally in both C++ and Python, perhaps it could live in sensor_msgs or similar. There's precedence for this (a message package providing extra API to work with certain messages).

Doesn't need to be done right this second, but it should be used here when it is made available.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yep, agreed


def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
int_marker = InteractiveMarker()
int_marker.header.frame_id = "base_link"
Expand Down Expand Up @@ -165,6 +173,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
normalizeQuaternion(control.orientation)
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
Expand All @@ -176,6 +185,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
normalizeQuaternion(control.orientation)
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
Expand All @@ -187,6 +197,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
normalizeQuaternion(control.orientation)
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
Expand All @@ -198,6 +209,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
normalizeQuaternion(control.orientation)
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
Expand All @@ -209,6 +221,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
normalizeQuaternion(control.orientation)
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
if fixed:
Expand All @@ -220,6 +233,7 @@ def make6DofMarker( fixed, interaction_mode, position, show_6dof = False):
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
normalizeQuaternion(control.orientation)
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
if fixed:
Expand Down Expand Up @@ -247,6 +261,7 @@ def makeRandomDofMarker( position ):
control.orientation.x = rand(-1,1)
control.orientation.y = rand(-1,1)
control.orientation.z = rand(-1,1)
normalizeQuaternion(control.orientation)
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(copy.deepcopy(control))
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
Expand Down Expand Up @@ -300,6 +315,7 @@ def makeQuadrocopterMarker(position):
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
normalizeQuaternion(control.orientation)
control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
int_marker.controls.append(copy.deepcopy(control))
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
Expand All @@ -321,6 +337,7 @@ def makeChessPieceMarker(position):
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
normalizeQuaternion(control.orientation)
control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
int_marker.controls.append(copy.deepcopy(control))

Expand Down Expand Up @@ -351,6 +368,7 @@ def makePanTiltMarker(position):
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
normalizeQuaternion(control.orientation)
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
int_marker.controls.append(control)
Expand All @@ -360,6 +378,7 @@ def makePanTiltMarker(position):
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
normalizeQuaternion(control.orientation)
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.INHERIT
int_marker.controls.append(control)
Expand Down Expand Up @@ -405,6 +424,7 @@ def makeMovingMarker(position):
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
normalizeQuaternion(control.orientation)
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(copy.deepcopy(control))

Expand Down
63 changes: 27 additions & 36 deletions interactive_marker_tutorials/src/basic_controls.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -226,32 +226,29 @@ void make6DofMarker( bool fixed, unsigned int interaction_mode, const tf::Vector

if(show_6dof)
{
control.orientation.w = 1;
control.orientation.x = 1;
control.orientation.y = 0;
control.orientation.z = 0;
tf::Quaternion orien(1.0, 0.0, 0.0, 1.0);
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);
control.name = "rotate_x";
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);
control.name = "move_x";
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);

control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 1;
control.orientation.z = 0;
orien = tf::Quaternion(0.0, 1.0, 0.0, 1.0);
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);
control.name = "rotate_z";
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);
control.name = "move_z";
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);

control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 0;
control.orientation.z = 1;
orien = tf::Quaternion(0.0, 0.0, 1.0, 1.0);
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);
control.name = "rotate_y";
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);
Expand Down Expand Up @@ -284,10 +281,9 @@ void makeRandomDofMarker( const tf::Vector3& position )

for ( int i=0; i<3; i++ )
{
control.orientation.w = rand(-1,1);
control.orientation.x = rand(-1,1);
control.orientation.y = rand(-1,1);
control.orientation.z = rand(-1,1);
tf::Quaternion orien(rand(-1,1), rand(-1,1), rand(-1,1), rand(-1,1));
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
Expand Down Expand Up @@ -354,10 +350,9 @@ void makeQuadrocopterMarker( const tf::Vector3& position )

InteractiveMarkerControl control;

control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 1;
control.orientation.z = 0;
tf::Quaternion orien(0.0, 1.0, 0.0, 1.0);
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);
control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE;
int_marker.controls.push_back(control);
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
Expand All @@ -381,10 +376,9 @@ void makeChessPieceMarker( const tf::Vector3& position )

InteractiveMarkerControl control;

control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 1;
control.orientation.z = 0;
tf::Quaternion orien(0.0, 1.0, 0.0, 1.0);
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);
control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
int_marker.controls.push_back(control);

Expand Down Expand Up @@ -417,18 +411,16 @@ void makePanTiltMarker( const tf::Vector3& position )

InteractiveMarkerControl control;

control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 1;
control.orientation.z = 0;
tf::Quaternion orien(0.0, 1.0, 0.0, 1.0);
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
control.orientation_mode = InteractiveMarkerControl::FIXED;
int_marker.controls.push_back(control);

control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 0;
control.orientation.z = 1;
orien = tf::Quaternion(0.0, 0.0, 1.0, 1.0);
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
control.orientation_mode = InteractiveMarkerControl::INHERIT;
int_marker.controls.push_back(control);
Expand Down Expand Up @@ -504,10 +496,9 @@ void makeMovingMarker( const tf::Vector3& position )

InteractiveMarkerControl control;

control.orientation.w = 1;
control.orientation.x = 1;
control.orientation.y = 0;
control.orientation.z = 0;
tf::Quaternion orien(1.0, 0.0, 0.0, 1.0);
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);

Expand Down
12 changes: 8 additions & 4 deletions interactive_marker_tutorials/src/pong.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@
#include <math.h>
#include <boost/thread/mutex.hpp>

#include <tf/tf.h>

using namespace visualization_msgs;

static const float FIELD_WIDTH = 12.0;
Expand Down Expand Up @@ -361,8 +363,9 @@ class PongGame
InteractiveMarkerControl control;
control.always_visible = false;
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
control.orientation.w = 1;
control.orientation.z = 1;
tf::Quaternion orien(0.0, 0.0, 1.0, 1.0);
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);

// Add a visualization marker
Marker marker;
Expand Down Expand Up @@ -442,8 +445,9 @@ class PongGame
int_marker.name = "ball";

control.interaction_mode = InteractiveMarkerControl::NONE;
control.orientation.w = 1;
control.orientation.y = 1;
tf::Quaternion orien(0.0, 1.0, 0.0, 1.0);
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);

Marker marker;
marker.color.r = 1.0;
Expand Down
21 changes: 11 additions & 10 deletions interactive_marker_tutorials/src/selection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
#include <interactive_markers/tools.h>

#include <tf/LinearMath/Vector3.h>
#include <tf/tf.h>

bool testPointAgainstAabb2(const tf::Vector3 &aabbMin1, const tf::Vector3 &aabbMax1,
const tf::Vector3 &point)
Expand Down Expand Up @@ -209,7 +210,7 @@ class PointCouldSelector
control.orientation_mode = vm::InteractiveMarkerControl::INHERIT;
control.always_visible = false;

control.orientation.w = 1;
tf::Quaternion orien;

switch ( axis )
{
Expand All @@ -218,27 +219,27 @@ class PointCouldSelector
int_marker.pose.position.x = sign>0 ? max_sel_.x() : min_sel_.x();
int_marker.pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
int_marker.pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
control.orientation.x = 1;
control.orientation.y = 0;
control.orientation.z = 0;
orien = tf::Quaternion(1.0, 0.0, 0.0, 1.0);
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);
break;
case 1:
int_marker.name = sign>0 ? "max_y" : "min_y";
int_marker.pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
int_marker.pose.position.y = sign>0 ? max_sel_.y() : min_sel_.y();
int_marker.pose.position.z = 0.5 * ( max_sel_.z() + min_sel_.z() );
control.orientation.x = 0;
control.orientation.y = 0;
control.orientation.z = 1;
orien = tf::Quaternion(0.0, 0.0, 1.0, 1.0);
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);
break;
default:
int_marker.name = sign>0 ? "max_z" : "min_z";
int_marker.pose.position.x = 0.5 * ( max_sel_.x() + min_sel_.x() );
int_marker.pose.position.y = 0.5 * ( max_sel_.y() + min_sel_.y() );
int_marker.pose.position.z = sign>0 ? max_sel_.z() : min_sel_.z();
control.orientation.x = 0;
control.orientation.y = -1;
control.orientation.z = 0;
orien = tf::Quaternion(0.0, -1.0, 0.0, 1.0);
orien.normalize();
tf::quaternionTFToMsg(orien, control.orientation);
break;
}

Expand Down