[robot] review rotation
来源:互联网 发布:mysql数据库最大容量 编辑:程序博客网 时间:2024/06/06 05:28
fundamental rotation
exercise
rotation around a fixed frame
rotation around an intermediate frame
representation of rotations
- basic rotations
- axis-angle notation
- quaternions
Compute Axis-angle based on Rotation Matrix
Convert a given rotation matrix to axis-angle representation
Question
A rotation matrix R can have two sets of solutions:
(vec, theta) or (-vec, -theta). These solutions are equivalent.
In this task, we will only test for theta in [0, pi], so you can ignore the other solution.
Singularities
You also need to handle two singularity cases, theta = 0 and theta = +pi.
(1). theta = 0 represents no rotation, and there are infinite solutions for this. So vec must be [NaN NaN NaN] in this case.
(2). theta = pi has two equivalent solutions. In this case, vec must be 2x3 matrix containing both solutions.
(3).For all other cases, vec will be a 1x3 matrix containing a single solution.
Input Format
R will be a valid 3x3 rotation matrix with det(R) = 1
Output Format
vec will be a 1x3 matrix containing a single solution if only one solution exists in [0, pi]
vec will be a 2x3 matrix containing two solutions, if two solutions exist in [0, pi]
vec will be a 1x3 matrix of the form [NaN, NaN, NaN] if infinite solutions exist in [0, pi]
Every row in vec is of the form [x y z] if the vector is
Unit
theta is always a 1x1 matrix, and in radians
The general form of the equation is:
However, it can be seen that the above equation for r is not valid when sin(theta) = 0
These are the singularity cases at theta = 0, theta = pi, theta = -pi which need to be handled separately
When theta = 0, we have infinite solutions, as it represents no rotation. So r can be any arbitrary vector.
The below equation should be useful in the theta = pi scenarios:
where,
function [vec, theta] = RotationMatrix2AxisAngle(R)%UNTITLED6 此处显示有关此函数的摘要% 此处显示详细说明temp = 0.5*trace(R) - 0.5;if abs(temp-1)< 1e-8 % theta = 0 theta = 0; vec = [NaN,NaN,NaN];elseif abs(temp + 1) < 1e-8 % theta = pi vec = zeros(2,3); theta = pi; rx = sqrt((R(1,1)+1)/2); if abs(rx) < 1e-8 % rx == 0 ry = sqrt(0.5*R(2,2)+0.5); if abs(ry) < 1e-8 rz = sqrt(0.5*R(3,3)+0.5); vec(1,:) = [rx,ry,rz]; vec(2,:) = [rx,ry,-rz]; else rz = R(3,2) / (2*ry); vec(1,:) = [rx,ry,rz]; ry = -sqrt(0.5*R(2,2)+0.5); rz = R(3,2) / (2*ry); vec(2,:) = [rx,ry,rz]; end else rz = (R(1,3)+R(3,1)) / (4*rx); ry = R(2,1)/(2*rx); vec(1,:) = [rx,ry,rz]; rx = -sqrt((R(1,1)+1)/2); rz = (R(1,3)+R(3,1)) / (4*rx); ry = R(2,1)/(2*rx); vec(2,:) = [rx,ry,rz]; endelse theta = acos(temp); vec = 0.5*[R(3,2)-R(2,3),R(1,3)-R(3,1),R(2,1)-R(1,2)]/sin(theta);endend
SLERP
question
write a function quat_slerp to perform SLERP between two quaternions and return all the intermediate quaternions.
All quaternions are represented as [Qs, Qx, Qy, Qz] in matrix form if the quaternion has scalar part Qs and vector part Qx i + Qy j + Qz k.
- q0 is the unit quaternion representing the starting orientation, 1x4 matrix
- q1 is the unit quaternion representing the final orientation, 1x4 matrix
- steps is the number of quaternions required to be returned, integer value
output
The first step is q0, and the last step is q1
q_int contains q0, steps-2 intermediate quaternions, q1 - in the order of ascending order of timesteps
q_int is a (steps x 4) matrix
hints
- Since the quaternions are unit quaternions, we can find the angle between them using dot product.
cos(θ)=q0⋅q1 - At time-step t, the intermediate quaternion is given as:
sin[(1−t)θ]sinθq0+sin[tθ]sinθq1
function [ q_int ] = quat_slerp( q0, q1, steps )%QUAT_SLERP Perform SLERP between two quaternions and return the intermediate quaternions% Usage: [ q_int ] = quat_slerp( q0, q1, steps )% Inputs:% q0 is the quaternion representing the starting orientation, 1x4 matrix% q1 is the quaternion representing the final orientation, 1x4 matrix% steps is the number of intermediate quaternions required to be returned, integer value% The first step is q0, and the last step is q1% Output:% q_int contains q0, steps-2 intermediate quaternions, q1% q_int is a (steps x 4) matrix q_int = zeros(steps, 4); cos_omega = q0*q1'; if cos_omega < 0 q1 = -q1; cos_omega = -cos_omega; end sin_omega = sqrt(1-cos_omega^2); omega = atan2(sin_omega, cos_omega); inv_sin_omega = 1.0 / sin_omega; num = (steps-1); for i=1:steps k0 = sin((1.0-(i-1)/num)*omega) * inv_sin_omega; k1 = sin((i-1)/num*omega) * inv_sin_omega; q_int(i,:) = k0 * q0 + k1 * q1; endend
- [robot] review rotation
- [robot] review forward kinematics
- Review
- Review
- review
- review
- review
- Review
- review
- Robot
- robot
- robot
- Robot
- Robot
- Rotation transformation
- 2.rotation
- string rotation
- IOS6 rotation
- linux下创建svn版本库
- JAVA集合类基础特性
- main主函数执行完毕后,是否会再执行一段代码?
- java List集合分页
- 5.12号学习日志
- [robot] review rotation
- 程序中中文乱码问题的总结
- Spring --单元测试及使用logback打印测试结果
- 手把手教你:android调用系统相机、相册功能,适配6.0权限获取以及7.0以后获取URI(兼容多版本)
- eclipse代码自动提示设置、如何配置eclipse的代码自动提示功能同时解决自动补全变量名的问题
- 如何编写一个framework
- 面试系列五
- Python 遇到的问题
- 如何更改UISlider的高度