本文整理汇总了C++中Rotation::qy方法的典型用法代码示例。如果您正苦于以下问题:C++ Rotation::qy方法的具体用法?C++ Rotation::qy怎么用?C++ Rotation::qy使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Rotation
的用法示例。
在下文中一共展示了Rotation::qy方法的13个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: rotateTowardRobot
void CameraController::rotateTowardRobot(Vector3d r_pos)
{
m_my->setPosition(m_rotatePos);
// カメラの位置を得る
m_my->getPosition(m_pos);
// カメラの位置からロボットを結ぶベクトル
Vector3d tmpp = r_pos;
tmpp -= m_pos;
// y方向は考えない
tmpp.y(0);
// カメラの回転角度を得る
Rotation myRot;
m_my->getRotation(myRot);
// カメラのy軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0)
theta = -1*theta;
// ロボットまでの回転角度を得る
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
// 角度差から回転量を得る
targetAngle += theta;
m_my->setAxisAndAngle(0, 1, 0, -targetAngle, 0);
}
示例2: rotateTowardObj
double DemoRobotController::rotateTowardObj(Vector3d pos)
{
// "pos" means target position
// get own position
Vector3d ownPosition;
m_robotObject->getPartsPosition(ownPosition,"RARM_LINK2");
// pointing vector for target
Vector3d l_pos = pos;
l_pos -= ownPosition;
// ignore variation on y-axis
l_pos.y(0);
// get own rotation matrix
Rotation ownRotation;
m_robotObject->getRotation(ownRotation);
// get angles arround y-axis
double qw = ownRotation.qw();
double qy = ownRotation.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0) theta = -1.0*theta;
// rotation angle from z-axis to x-axis
double tmp = l_pos.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// direction
if(l_pos.x() > 0) targetAngle = -1.0*targetAngle;
targetAngle += theta;
double angVelFac = 3.0;
double l_angvel = m_angularVelocity/angVelFac;
if(targetAngle == 0.0) {
return 0.0;
}
else {
// circumferential distance for the rotation
double l_distance = m_distance*M_PI*fabs(targetAngle)/(2.0*M_PI);
// Duration of rotation motion (micro second)
double l_time = l_distance / (m_movingSpeed/angVelFac);
// Start the rotation
if(targetAngle > 0.0) {
m_robotObject->setWheelVelocity(l_angvel, -l_angvel);
}
else{
m_robotObject->setWheelVelocity(-l_angvel, l_angvel);
}
return l_time;
}
}
示例3: getRoll
double RobotController::getRoll(Rotation rot)
{
// get angles arround y-axis
double qw = rot.qw();
double qx = rot.qx();
double qy = rot.qy();
double qz = rot.qz();
double roll = atan2(2*qy*qw - 2*qx*qz, 1 - 2*qy*qy - 2*qz*qz);
return roll;
}
示例4: calcHeadingAngle
double MyController::calcHeadingAngle()
{
// 自分の回転を得る
Rotation myRot;
m_my->getRotation(myRot);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if (qw * qy < 0) {
theta = -1 * theta;
}
return theta * 180.0 / PI;
}
示例5: rotateTowardObj
double MyController::rotateTowardObj(Vector3d pos)
{
// 自分の位置の取得
Vector3d myPos;
my->getPosition(myPos);
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpp = pos;
tmpp -= myPos;
// y方向は考えない
tmpp.y(0);
// 自分の回転を得る
Rotation myRot;
my->getRotation(myRot);
// エンティティの初期方向
Vector3d iniVec(0.0, 0.0, 1.0);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0)
theta = -1*theta;
// z方向からの角度
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// 方向
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
targetAngle += theta;
return targetAngle;
}
示例6: getPrimitivesFromRSB
void getPrimitivesFromRSB(boost::shared_ptr<Primitive3DFloatSet> evPrimitiveSet){
std::vector<Primitive3DFilter::Primitive3D> rsbPrimitives;
for(int i = 0; i < evPrimitiveSet->primitives_size(); ++i) {
Primitive3DFloat primitiveProto = evPrimitiveSet->primitives(i);
Primitive3DFilter::PrimitiveType primitiveType = Primitive3DFilter::CUBE;
Primitive3DFloat_PrimitiveType typeRST = primitiveProto.type();
if(typeRST == Primitive3DFloat_PrimitiveType_CUBE)
primitiveType = Primitive3DFilter::CUBE;
else if(typeRST == Primitive3DFloat_PrimitiveType_SPHERE)
primitiveType = Primitive3DFilter::SPHERE;
else if(typeRST == Primitive3DFloat_PrimitiveType_CYLINDER)
primitiveType = Primitive3DFilter::CYLINDER;
Translation positionProto = primitiveProto.pose().translation();
Vec primitivePosition(positionProto.x()*1000.0, positionProto.y()*1000.0, positionProto.z()*1000.0, 1);
Rotation rotation = primitiveProto.pose().rotation();
Primitive3DFilter::Quaternion primitiveOrientation(Vec3(rotation.qx(), rotation.qy(), rotation.qz()), rotation.qw());
Vec primitiveScale(primitiveProto.scale().x()*1000.0, primitiveProto.scale().y()*1000.0, primitiveProto.scale().z()*1000.0, 1);
Primitive3DFilter::Primitive3D primitive(primitiveType, primitivePosition, primitiveOrientation, primitiveScale, 0, primitiveProto.description());
rsbPrimitives.push_back(primitive);
}
primitivesMutex.lock();
primitives = rsbPrimitives;
primitivesMutex.unlock();
}
示例7: rotateTowardObj
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpp = pos;
tmpp -= myPos;
// y方向は考えない
tmpp.y(0);
// 自分の回転を得る
Rotation myRot;
m_my->getRotation(myRot);
// エンティティの初期方向
Vector3d iniVec(0.0, 0.0, 1.0);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0)
theta = -1*theta;
// z方向からの角度
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// 方向
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
targetAngle += theta;
if(targetAngle == 0.0){
return 0.0;
}
else {
// 回転すべき円周距離
double distance = m_distance*PI*fabs(targetAngle)/(2*PI);
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
// 回転時間(u秒)
double time = distance / vel;
// 車輪回転開始
if(targetAngle > 0.0){
m_my->setWheelVelocity(velocity, -velocity);
}
else{
m_my->setWheelVelocity(-velocity, velocity);
}
return now + time;
}
}
示例8: onAction
double MyController::onAction(ActionEvent &evt) {
int actionNumber = 2;
int functionalFeature = 1;
int targetType = 3;
myfile << setprecision(2) << std::fixed;
// handle of target and tool
SimObj *target = getObj("box_004");
SimObj *toolName = getObj("TShapeTool_004");
if (evt.time() < 5.0)
{
// cout << "Time" << endl;
cout << evt.time() << endl;
toolName->getPosition(currentToolPos); // get the current tool position
toolName->getRotation(finalToolRotation);
toolName->getVelocity(finalToolVel);
isToolAtRest = checkEntityMotionStatus(toolName); // checks whether the tool is moving by calculating its velocity
target->getPosition(currentTargetPos);
target->getRotation(finalTargetRotation);
target->getVelocity(finalTargetVel);
isTargetAtRest = checkEntityMotionStatus(target); // checks whether the object is moving by calculating its velocity
}
if (evt.time() > 5.0)
{
insideTimer = false;
counterOfAction ++ ;
}
if(!insideTimer && counterOfAction == 1 )
{
myfile << actionNumber << " , " << functionalFeature << " , " ;
myfile << initToolRotation.qw() << " , " << initToolRotation.qx() << " , " << initToolRotation.qy() << " , " << initToolRotation.qz() << " , " ;
myfile << initTargetRotation.qw() << " , " << initTargetRotation.qx() << " , " << initTargetRotation.qy() << " , " << initTargetRotation.qz() << " , " ;
myfile << finalTargetRotation.qw() << " , " << finalTargetRotation.qx() << " , " << finalToolRotation.qy() << " , " << finalToolRotation.qz() << " , " ;
myfile << initToolPos.x() << " , " << initToolPos.z() << " , " ;
myfile << initTargetPos.x() << " , " << initTargetPos.z() << " , " ;
myfile << forceOnTool_x << " , " << forceOnTool_z << " , " ;
myfile << appliedToolVel.x() << " , " << appliedToolVel.z() << " , " ;
myfile << toolVelAtHit.x() << " , " << toolVelAtHit.z() << " , " ;
myfile << targetVelAtHit.x() << " , " << targetVelAtHit.z() << " , " ;
myfile << currentToolPos.x() << " , " << currentToolPos.z() << " , " ;
myfile << currentTargetPos.x() << " , " << currentTargetPos.z() << " , " ;
myfile << finalToolVel.x() << " , " << finalToolVel.z() << " , " ;
myfile << finalTargetVel.x() << " , " << finalTargetVel.z() << " , " ;
myfile << isToolAtRest << " , " << isTargetAtRest << " , " ;
myfile << currentToolPos.x() - initToolPos.x() << " , " << currentToolPos.z() - initToolPos.z() << " , " ;
myfile << currentTargetPos.x() - initTargetPos.x() << " , " << currentTargetPos.z() - initTargetPos.z();
myfile << "\n";
cout << "The simulation for " << actionNumber << " , " << functionalFeature << " has been recorded" << endl;
// exit(0);
flag = false;
}
return 0.01;
}
示例9: rotateTowardGrabPos
double MyController::rotateTowardGrabPos(Vector3d pos, double velocity, double now)
{
printf("start rotate %lf \n", now);
//自分を取得
SimObj *my = getObj(myname());
//printf("向く座標 %lf %lf %lf \n", pos.x(), pos.y(), pos.z());
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
//自分の手のパーツを得ます
CParts * parts = my->getParts("RARM_LINK7");
Vector3d partPos;
parts->getPosition(partPos);
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpp = pos;
//tmpp -= myPos;
tmpp -= partPos;
// y方向は考えない
tmpp.y(0);
// 自分の回転を得る
Rotation myRot;
m_my->getRotation(myRot);
// エンティティの初期方向
Vector3d iniVec(0.0, 0.0, 1.0);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
//printf("qw: %lf theta: %lf \n", qw, theta);
if(qw*qy < 0) {
//printf("qw * qy < 0 \n");
theta = -1*theta;
}
// z方向からの角度
//printf("結ぶベクトル 座標 %lf %lf %lf \n", tmpp.x(), tmpp.y(), tmpp.z());
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
//printf("tmp: %lf \n", tmp);
double targetAngle = acos(tmp);
//printf("targetAngle: %lf ---> %lf \n", targetAngle, targetAngle*180.0/PI);
// 方向
//printf("tmpp.x() %lf \n", tmpp.x());
if(tmpp.x() > 0) {
targetAngle = -1*targetAngle;
//printf("targetAngle: %lf deg \n", targetAngle*180.0/PI);
}
targetAngle += theta;
//printf("targetAngle: %lf <--- %lf \n", targetAngle, theta);
//printf("qw: %lf qy: %lf theta: %lf tmp: %lf targetAngle: %lf \n", qw, qy, theta, tmp, targetAngle);
if(targetAngle == 0.0){
//printf("donot need rotate \n");
return 0.0;
}
else
{
// 回転すべき円周距離
double distance = m_distance*PI*fabs(targetAngle)/(2*PI);
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
// 回転時間(u秒)
double time = distance / vel;
// 車輪回転開始
if(targetAngle > 0.0){
m_my->setWheelVelocity(velocity, -velocity);
}
else
{
m_my->setWheelVelocity(-velocity, velocity);
}
//printf("distance: %lf vel: %lf time: %lf \n", distance, vel, time);
printf("rotate time: %lf, time to stop: %lf \n", time, now + time);
return now + time;
}
}
示例10: rotateTowardObj
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
// 自分の回転を得る
Rotation myRot;
m_my->getRotation(myRot);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2 * acos(fabs(qw));
if (qw * qy < 0) {
theta = -1 * theta;
}
printf("ロボットが向いている角度 current theta: %lf(deg) \n", theta * 180 / PI);
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
printf("ロボットの現在位置: x: %lf, z %lf \n", myPos.x(), myPos.z());
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpPos = pos;
tmpPos -= myPos;
// 近すぎるなら,回転なし
double dis = tmpPos.x() * tmpPos.x() + tmpPos.z() * tmpPos.z();
if (dis < 1.0) {
return 0.0;
}
// z方向からの角度
double rate = tmpPos.x() / tmpPos.z();
double targetAngle = atan(rate);
if (tmpPos.z() < 0) {
targetAngle += PI;
}
printf("回転する角度 targetAngle: %lf(deg) 結ぶベクトル tmpPos.x: %lf, tmpPos.z: %lf, rate: %lf \n", targetAngle*180.0/PI, tmpPos.x(), tmpPos.z(), rate);
targetAngle -= theta;
if (targetAngle > PI) {
targetAngle = targetAngle - 2 * PI;
}
printf("targetAngle: %lf(deg) currentAngle: %lf(deg) \n", targetAngle*180.0/PI, theta * 180.0 / PI);
if (targetAngle == 0.0) {
printf("donot need to rotate \n");
return 0.0;
} else {
// 回転すべき円周距離
double distance = m_distance * PI * fabs(targetAngle) / (2 * PI);
// 車輪の半径から移動速度を得る
double vel = m_radius * velocity;
// 回転時間(u秒)
double time = distance / vel;
printf("rotateTime: %lf \n", time);
// 車輪回転開始
if (targetAngle > 0.0) {
m_my->setWheelVelocity(-velocity, velocity);
} else {
m_my->setWheelVelocity(velocity, -velocity);
}
return now + time;
}
}
示例11: rotateTowardObj
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
// get own position
Vector3d myPos;
m_my->getPosition(myPos);
// vector from own position to a target position
Vector3d tmpp = pos;
tmpp -= myPos;
// rotation about y-axis is always 0
tmpp.y(0);
// get own rotation
Rotation myRot;
m_my->getRotation(myRot);
// initial direction
Vector3d iniVec(0.0, 0.0, 1.0);
// get rotation angle about y-axis
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0){
theta = -theta;
}
// angle from z-axis
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// calcurate target angle
if(tmpp.x() > 0){
targetAngle = -1*targetAngle;
}
targetAngle += theta;
if(targetAngle<-M_PI){
targetAngle += 2*M_PI;
}
else if(targetAngle>M_PI){
targetAngle -= 2*M_PI;
}
if(targetAngle == 0.0){
return 0.0;
}
else {
// circumference length for rotation
double distance = m_distance*M_PI*fabs(targetAngle)/(2*M_PI);
// calcurate velocity from radius of wheels
double vel = m_radius*velocity;
// rotation time (micro second)
double time = distance / vel;
// start rotating
if(targetAngle > 0.0){
m_my->setWheelVelocity(velocity, -velocity);
}
else{
m_my->setWheelVelocity(-velocity, velocity);
}
return now + time;
}
}
示例12: onAction
//.........这里部分代码省略.........
for(std::map<std::string, CParts *>::iterator it = stick->getPartsCollection().begin(); it != stick->getPartsCollection().end(); ++it){
if (it->first != "body")
s.push_back(it->first);
}
std::string linkName;
Size si;
cout << "The total links are " << s.size() << endl;
for (int i = 0; i < s.size(); i++){
const char* linkName = s[i].c_str();
CParts *link = stick->getParts(linkName);
link->getPosition(pos);
link->getRotation(linkRotation);
if (link->getType() == 0){
BoxParts* box = (BoxParts*) link;
si = box->getSize();
// cout << linkName << endl;
cout << linkName << " position : x = " << pos.x() << " y = " << pos.y() << " z = " << pos.z() << endl;
// cout << linkName << " size : x = " << si.x() << " y = " << si.y() << " z = " << si.z() << endl;
// cout << linkName << " Rotation: qw " << linkRotation.qw() << " qx = "<< linkRotation.qx() << " qy = "<< linkRotation.qy()
// << " qz = "<< linkRotation.qz() << endl;
try{
py::object main_module = py::import("__main__");
py::object main_namespace = main_module.attr("__dict__");
main_module.attr("linkName") = linkName;
main_module.attr("length") = si.x();
main_module.attr("height") = si.y();
main_module.attr("breadth") = si.z();
main_module.attr("linkPos") = "[" + tostr(pos.x())+" , "+ tostr(pos.y())+ " , " + tostr(pos.z()) + "]";
main_module.attr("rotation") = "[" + tostr(linkRotation.qw())+" , "+ tostr(linkRotation.qx())+ " , " + tostr(linkRotation.qy()) + " , " + tostr(linkRotation.qz()) +"]";
// calculating the rotation of the tool.
py::exec("import ast", main_namespace);
py::exec("import transformations as T", main_namespace);
py::exec("rotation = ast.literal_eval(rotation)", main_namespace);
// py::exec("angles = T.euler_from_quaternion(rotation) ", main_namespace);
// py::exec("print angles", main_namespace);
py::exec("linkPos = ast.literal_eval(linkPos)", main_namespace);
py::exec_file("vertices.py", main_namespace, main_namespace );
py::exec("getNormals(linkName, linkPos, rotation, length, height, breadth)", main_namespace);
main_module.attr("targetPos") = "[" + tostr(targetPos.x())+" , "+ tostr(targetPos.y())+ " , " + tostr(targetPos.z()) + "]";
main_module.attr("displacementVect") = "[" + tostr(displacementVect.x())+" , "+ tostr(displacementVect.y())+ " , " + tostr(displacementVect.z()) + "]";
py::exec_file("displacementVector.py", main_namespace, main_namespace );
}
catch(boost::python::error_already_set const &){
// Parse and output the exception
std::string perror_str = parse_python_exception();
std::cout << "Error in Python: " << perror_str << std::endl;
}
}
else if(link->getType() == 1){
CylinderParts* cyl = (CylinderParts*) link;
cout << "Cylinder Position : x = " << pos.x() << " y = " << pos.y() << " z = " << pos.z() << endl;
cout << "Cylinder Length : length = " << cyl->getLength() << endl;
cout << "Cylinder Radius : rad = " << cyl->getRad() << endl;
}
else if(link->getType() == 2){
SphereParts* sph = (SphereParts*) link;
cout << "Sphere Position : x = " << pos.x() << " y = " << pos.y() << " z = " << pos.z() << endl;
cout << "Sphere Radius : rad = " << sph->getRad() << endl;
}
}
// stick->getPartsPosition(pos, s[1]);
// stick->getPartsPosition(pos, "LINK1");
// LOG_MSG((" LINK1 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() ));
// stick->getPartsPosition(pos, "LINK2");
// LOG_MSG((" LINK2 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() ));
// stick->getPartsPosition(pos, "LINK3");
// LOG_MSG((" LINK3 Position is : %f %f %f ", pos.x(), pos.y(), pos.z() ));
return 0.00001;
}
示例13: rotateTowardObj
double MyController::rotateTowardObj(Vector3d pos, double velocity, double now)
{
// 自分の位置の取得
Vector3d myPos;
m_my->getPosition(myPos);
// 自分の位置からターゲットを結ぶベクトル
Vector3d tmpp = pos;
tmpp -= myPos;
// y方向は考えない
tmpp.y(0);
// 自分の回転を得る
Rotation myRot;
m_my->getRotation(myRot);
printf("ロボットの現在位置: x: %lf, z %lf \n", myPos.x(), myPos.z());
// エンティティの初期方向
Vector3d iniVec(0.0, 0.0, 1.0);
// y軸の回転角度を得る(x,z方向の回転は無いと仮定)
double qw = myRot.qw();
double qy = myRot.qy();
double theta = 2*acos(fabs(qw));
if(qw*qy < 0)
theta = -1*theta;
printf("ロボットが向いている角度 current theta: %lf(deg) \n", theta * 180 / PI);
// z方向からの角度
double tmp = tmpp.angle(Vector3d(0.0, 0.0, 1.0));
double targetAngle = acos(tmp);
// 方向
if(tmpp.x() > 0) targetAngle = -1*targetAngle;
targetAngle += theta;
printf("targetAngle: %lf(deg) currentAngle: %lf(deg) \n", targetAngle*180.0/PI, theta * 180.0 / PI);
if(targetAngle == 0.0){
return 0.0;
}
else {
// 回転すべき円周距離
double distance = m_distance*PI*fabs(targetAngle)/(2*PI);
printf("distance: %lf \n", distance);
// 車輪の半径から移動速度を得る
double vel = m_radius*velocity;
printf("radius: %lf, velocity: %lf, vel: %lf \n", m_radius, velocity, vel);
// 回転時間(u秒)
double time = distance / vel;
printf("rotateTime: %lf = dis: %lf / vel: %lf\n", time, distance, vel);
// 車輪回転開始
if(targetAngle > 0.0){
m_my->setWheelVelocity(velocity, -velocity);
}
else{
m_my->setWheelVelocity(-velocity, velocity);
}
return now + time;
}
}