void S_position_offset_list(double posarray[], double grad, double nslices,
double resfrq, int device, int listno, codeint apv1)
{
int k;
double freqarray[MAXSLICE], nucgamma;
if (nslices <= 1)
position_offset(posarray[0],grad,resfrq,device);
else {
nucgamma = nucleus_gamma(device);
for (k=0; k<(int)nslices; k++) {
freqarray[k] = resfrq + nucgamma*grad*posarray[k];
}
create_offset_list(freqarray,(int)nslices,device,listno);
voffset(listno,apv1);
}
}
geometry_msgs::PoseStamped PSMoveTemplateController::updatePose(geometry_msgs::PoseStamped object_pose, MoveServerPacket *move_server_packet, float normalized_scale)
{
geometry_msgs::PoseStamped transformed_pose;
/////////////////////////
//Calculate scale
QVector3D cam(camera_position_);
QVector3D obj(object_pose.pose.position.x,object_pose.pose.position.y,object_pose.pose.position.z);
double distance = (cam - obj).length();
//ROS_ERROR("distance: %f vfov: %f center distance: %f", distance, camera_update_.vfov, cos(camera_update_.vfov) * distance);
double scale = (cos(camera_update_.vfov) * distance) * 1.0 / 5.0; // distance=5m is scale 1.0
float rotation_scale = normalized_scale;// * scale;
float position_scale = 0.01 * normalized_scale * scale; // move position reported in mm, so scale it down (right now it's 10x real-world scale)
/////////////////////////
//Translation- move relative to camera for direction but still translate in world space
// create a vector representing the offset to be applied to position.
QVector3D position_offset((move_server_packet->state[0].handle_pos[0] - old_move_position_.x()) * position_scale,
(move_server_packet->state[0].handle_pos[1] - old_move_position_.y()) * position_scale,
(move_server_packet->state[0].handle_pos[2] - old_move_position_.z()) * position_scale);
//rotate the offset to be relative to camera orientation, can now be applied to position to move relative to camera
QVector3D rotated_position_offset = camera_orientation_.rotatedVector(position_offset);
//Update x, y, and z values
// NEED TO RESTORE THESE FOR POSITION CONTROL
if(first_time_)
{
transformed_pose.pose.position.x = object_pose.pose.position.x;
transformed_pose.pose.position.y = object_pose.pose.position.y;
transformed_pose.pose.position.z = object_pose.pose.position.z;
}
else
{
transformed_pose.pose.position.x = object_pose.pose.position.x;// + rotated_position_offset.x();
transformed_pose.pose.position.y = object_pose.pose.position.y;// + rotated_position_offset.y();
transformed_pose.pose.position.z = object_pose.pose.position.z;// + rotated_position_offset.z();
}
/////////////////////////
//Rotation
// calculate the relative rotation from the old move orientation, so we only have the difference
QQuaternion move_orientation(move_server_packet->state[0].quat[3],move_server_packet->state[0].quat[0],move_server_packet->state[0].quat[1],move_server_packet->state[0].quat[2]);
QQuaternion move_orientation_offset = old_move_orientation_.conjugate() * move_orientation;
// object orientation
QQuaternion object_orientation(object_pose.pose.orientation.w,object_pose.pose.orientation.x,object_pose.pose.orientation.y,object_pose.pose.orientation.z);
camera_orientation_.normalize();
move_orientation_offset.normalize();
object_orientation.normalize();
printf("DEBUG\n");
//printf(" cam: %.3f, %.3f, %.3f, %.3f\n",camera_orientation_.scalar(),camera_orientation_.x(),camera_orientation_.y(),camera_orientation_.z());
//printf(" move1: %.3f, %.3f, %.3f, %.3f\n",move_orientation_offset.scalar(),move_orientation_offset.x(),move_orientation_offset.y(),move_orientation_offset.z());
//printf(" obj: %.3f, %.3f, %.3f, %.3f\n",object_orientation.scalar(),object_orientation.x(),object_orientation.y(),object_orientation.z());
float x,y,z;
quatToEuler(camera_orientation_,y,z,x);
printf(" cam: %.3f, %.3f, %.3f\n",x,y,z);
quatToEuler(move_orientation_offset,y,z,x);
printf(" move1: %.3f, %.3f, %.3f\n",x,y,z);
quatToEuler(object_orientation,y,z,x);
printf(" obj: %.3f, %.3f, %.3f\n",x,y,z);
if(first_time_)
{
// now we need to apply the orientation offset relative to the camera view
// QQuaternion camera_T_move = camera_orientation_ * move_orientation;
// QQuaternion object_T_camera = last_object_T_move_.conjugate() * camera_orientation_;
// QQuaternion object_T_move = object_T_camera * camera_T_move;
// camera_T_object = camera_orientation_.conjugate() * object_orientation;
degrees_ = 0;
}
// QQuaternion rotated_orientation = move_orientation * last_object_T_move_.conjugate();
///////
// old stuff - not quite right, doesn't take starting pose of move into consideration
//based on http://www.arcsynthesis.org/gltut/Positioning/Tut08%20Camera%20Relative%20Orientation.html
//new_rotation_offset = camera_orientation^-1 * (rotation_offset * camera_orientation)
//QQuaternion rotated_orientation_offset = camera_orientation_ * move_orientation_offset * camera_orientation_.conjugate() * object_orientation;
QQuaternion rotated_orientation_offset = camera_orientation_ * QQuaternion::fromAxisAndAngle(QVector3D(1,0,0),1) * camera_orientation_.conjugate() * object_orientation; // making transform constant
rotated_orientation_offset = camera_orientation_ * move_orientation * camera_orientation_.conjugate();
quatToEuler(rotated_orientation_offset,y,z,x);
printf(" off: %.3f, %.3f, %.3f\n",x,y,z);
///////
// based on matrices
// calculate camera x axis
// QVector3D camera_x_axis = camera_orientation_.rotatedVector(QVector3D(1,0,0));
// move_orientation_offset = QQuaternion::fromAxisAndAngle(camera_x_axis,degrees_++);//camera_orientation_ * move_orientation_offset;
// QQuaternion camera_T_new_object = move_orientation_offset * camera_T_object;
// QQuaternion rotated_orientation_offset = camera_orientation_ * camera_T_new_object;
///////
// based on vectors
//Gets the world vector space for cameras vectors
QVector3D camera_x = camera_orientation_.rotatedVector(QVector3D(1,0,0));
QVector3D camera_y = camera_orientation_.rotatedVector(QVector3D(0,1,0));
QVector3D camera_z = camera_orientation_.rotatedVector(QVector3D(0,0,1));
printf(" camera X: %.3f, %.3f, %.3f\n",camera_x.x(),camera_x.y(),camera_x.z());
printf(" camera Y: %.3f, %.3f, %.3f\n",camera_y.x(),camera_y.y(),camera_y.z());
printf(" camera Z: %.3f, %.3f, %.3f\n",camera_z.x(),camera_z.y(),camera_z.z());
QVector3D move_x = move_orientation.rotatedVector(QVector3D(1,0,0));
QVector3D move_y = move_orientation.rotatedVector(QVector3D(0,1,0));
QVector3D move_z = move_orientation.rotatedVector(QVector3D(0,0,1));
printf(" move X: %.3f, %.3f, %.3f\n",move_x.x(),move_x.y(),move_x.z());
printf(" move Y: %.3f, %.3f, %.3f\n",move_y.x(),move_y.y(),move_y.z());
printf(" move Z: %.3f, %.3f, %.3f\n",move_z.x(),move_z.y(),move_z.z());
// //Turns relative vectors from world to objects local space
// QVector3D object_relative_y = object_orientation.conjugate().rotatedVector(camera_y);
// QVector3D object_relative_x = object_orientation.conjugate().rotatedVector(camera_x);
// QVector3D object_relative_z = object_orientation.conjugate().rotatedVector(camera_z);
// QQuaternion rotated_orientation_offset = QQuaternion::fromAxisAndAngle(object_relative_z,1)
// * QQuaternion::fromAxisAndAngle(object_relative_y,0)
// * QQuaternion::fromAxisAndAngle(object_relative_x,0);
rotated_orientation_offset.normalize();
// DEBUG
// ROS_ERROR("DEBUG");
// float x,y,z;
// quatToEuler(object_orientation,y,x,z);
// ROS_ERROR(" obj: %.3f, %.3f, %.3f",x,y,z);
// quatToEuler(move_orientation_offset,y,x,z);
// ROS_ERROR(" off: %.3f, %.3f, %.3f",x,y,z);
// quatToEuler(rotated_orientation_offset,y,x,z);
// ROS_ERROR(" rot: %.3f, %.3f, %.3f",x,y,z);
// DEBUG
QQuaternion transformed_orientation;
//if(first_time_)
// transformed_orientation = object_orientation;
//else
transformed_orientation = rotated_orientation_offset;
transformed_pose.pose.orientation.w = transformed_orientation.scalar();
transformed_pose.pose.orientation.x = transformed_orientation.x();
transformed_pose.pose.orientation.y = transformed_orientation.y();
transformed_pose.pose.orientation.z = transformed_orientation.z();
object_pose_.pose.orientation.w = transformed_orientation.scalar();
object_pose_.pose.orientation.x = transformed_orientation.x();
object_pose_.pose.orientation.y = transformed_orientation.y();
object_pose_.pose.orientation.z = transformed_orientation.z();
/////////////////////////
//Header
transformed_pose.header = object_pose.header;
transformed_pose.header.stamp = ros::Time::now();
first_time_ = false;
return transformed_pose;
}
