Needle::Needle(const Vector3d& pos, const Matrix3d& rot, double degrees, double r, float c0, float c1, float c2, World* w, ThreadConstrained* t, int constrained_vertex_num)
: EnvObject(c0, c1, c2, NEEDLE)
, angle(degrees)
, radius(r)
, thread(t)
, constraint(constrained_vertex_num)
, world(w)
, position_offset(0.0, 0.0, 0.0)
, rotation_offset(Matrix3d::Identity())
{
assert(((thread == NULL) && (constrained_vertex_num == -1)) || ((thread != NULL) && (constrained_vertex_num != -1)));
updateConstraintIndex();
assert(degrees > MIN_ANGLE);
assert(r > MIN_RADIUS);
double arc_length = radius * angle * M_PI/180.0;
int pieces = ceil(arc_length/2.0);
for (int piece=0; piece<pieces; piece++) {
Intersection_Object* obj = new Intersection_Object();
obj->_radius = radius/8.0;
i_objs.push_back(obj);
}
if (thread != NULL) {
Matrix3d new_rot = AngleAxisd(-M_PI/2.0, rot.col(0)) * (AngleAxisd((angle) * M_PI/180.0, rot.col(1)) * rot);
setTransform(pos - radius * rot.col(1), new_rot);
} else {
setTransform(pos, rot);
}
}
RTC::ReturnCode_t CoordinateTransformer::onActivated(RTC::UniqueId ec_id)
{
//initialize
m_InputV << 0, 0, 0;
m_OutputV << 0, 0, 0;
//deg ⇒ rad
m_rot_degX = m_rot_degX* M_PI / 180.0;
m_rot_degY = m_rot_degY* M_PI / 180.0;
m_rot_degZ = m_rot_degZ* M_PI / 180.0;
//各軸回転クオータニオン
m_qX = AngleAxisd(m_rot_degX, Vector3d::UnitX());
m_qY = AngleAxisd(m_rot_degY, Vector3d::UnitY());
m_qZ = AngleAxisd(m_rot_degZ, Vector3d::UnitZ());
//平行移動
m_trans = Translation3d(m_transX, m_transY, m_transZ);
if (m_mirrorXY){
m_scaleZ *= -1;
}
if (m_mirrorYZ){
m_scaleX *= -1;
}
if (m_mirrorZX){
m_scaleY *= -1;
}
return RTC::RTC_OK;
}
Matrix3d FromEigenEulerAngleToEigenMatrix(const Vector3d& eulerAngle)
{
Matrix3d rot;
rot = AngleAxisd(eulerAngle[0], Vector3d::UnitZ())
* AngleAxisd(eulerAngle[1], Vector3d::UnitX())
* AngleAxisd(eulerAngle[2], Vector3d::UnitY());
return rot;
}
Quaterniond cOrbit::RotationToReferenceFrame() const
{
Vector3d axisOfInclination(std::cos(-ArgumentOfPeriapsis()), std::sin(-ArgumentOfPeriapsis()), 0);
// not tested at nonzero inclinations
return
AngleAxisd(- LongitudeOfAscendingNode() - ArgumentOfPeriapsis(), Vector3d::UnitZ()) *
AngleAxisd(- Inclination(), axisOfInclination);
}
void Needle::updateTransformFromAttachment()
{
if (isThreadAttached()) {
const Matrix3d thread_rot = thread->rotationAtConstraint(constraint_ind);
const Vector3d thread_pos = thread->positionAtConstraint(constraint_ind);
const Matrix3d needle_rot = AngleAxisd(angle * M_PI/180.0, thread_rot.col(1)) * thread_rot;
const Vector3d needle_pos = thread_pos - radius * (AngleAxisd(-angle * M_PI/180.0, needle_rot.col(1)) * needle_rot).col(2);
setTransform(needle_pos, needle_rot);
}
}
void Camera::update_pose() {
Matrix3d m;
m = AngleAxisd(yaw_, Vector3d::UnitZ())
* AngleAxisd(pitch_, Vector3d::UnitY())
* AngleAxisd(roll_, Vector3d::UnitX());
pose_.setIdentity();
pose_ *= m;
Vector3d v;
v << x_, y_, z_;
pose_.translation() = v;
}
//pos and rot correspond to the needle transform
void Needle::setTransform(const Vector3d& pos, const Matrix3d& rot)
{
position = pos;
rotation = rot;
double angle_per_link = angle/i_objs.size();
i_objs[0]->_start_pos = position + radius * rotation.col(2);
i_objs[0]->_end_pos = position + radius * (rotation * AngleAxisd(-angle_per_link * M_PI/180.0, Vector3d::UnitY())).col(2);
for (int piece=1; piece<i_objs.size(); piece++) {
i_objs[piece]->_start_pos = i_objs[piece-1]->_end_pos;
i_objs[piece]->_end_pos = position + radius * (rotation * AngleAxisd(-angle_per_link * (piece+1) * M_PI/180.0, Vector3d::UnitY())).col(2);
}
}
// *******************************************************
// display(): called once per frame, whenever OpenGL decides it's time to redraw.
virtual void display( float animation_delta_time, Vector2d &window_steering )
{
if( animate ) animation_time += animation_delta_time;
update_camera( animation_delta_time , window_steering );
int basis_id = 0;
float atime = animation_time * 10;
Matrix4d model_transform = Matrix4d::Identity();
Matrix4d std_model = model_transform;
*m_tree = Tree(Matrix4d::Identity(), atime);
// Start coding here!!!!
m_bee->timepassby(atime);
// Plane
model_transform = std_model * Affine3d(Translation3d(5, -5, -60)).matrix(); // Position
glUniform4fv(g_addrs->color_loc, 1, Vector4f(.0f, .7f, .9f, 1).data()); // Color
m_plane->draw ( projection_transform, camera_transform, model_transform, "" );
// Tree
model_transform = std_model * Affine3d(Translation3d(4, -5, -40)).matrix(); // Position
glUniform4fv( g_addrs->color_loc, 1, Vector4f( .0f, .6f ,.2f ,1 ).data()); // Color
m_tree->draw( basis_id++, projection_transform, camera_transform, model_transform, "");
// Leg
model_transform = std_model * Affine3d(Translation3d(4, 1+2*(abs(20.0 - ((int)atime % 41)))/10, -40)).matrix(); // Position
model_transform *= Affine3d(AngleAxisd((-PI / 60 * atime), Vector3d(0, 1, 0))).matrix();
model_transform *= Affine3d(Translation3d(20, 0, 0)).matrix();
m_bee->draw(basis_id++, projection_transform, camera_transform, model_transform, "");
}
/*
void ContactDynamics::updateNormalMatrix() {
static Timer t1("t1");
static Timer t2("t2");
static Timer t3("t3");
mN = MatrixXd::Zero(getNumTotalDofs(), getNumContacts());
for (int i = 0; i < getNumContacts(); i++) {
ContactPoint& c = mCollisionChecker->getContact(i);
Vector3d p = c.point;
int skelID1 = mBodyIndexToSkelIndex[c.bdID1];
int skelID2 = mBodyIndexToSkelIndex[c.bdID2];
if (!mSkels[skelID1]->getImmobileState()) {
int index1 = mIndices[skelID1];
int NDOF1 = c.bd1->getSkel()->getNumDofs();
Vector3d N21 = c.normal;
MatrixXd J21 = getJacobian(c.bd1, p);
mN.block(index1, i, NDOF1, 1) = J21.transpose() * N21;
}
if (!mSkels[skelID2]->getImmobileState()) {
t1.startTimer();
int index2 = mIndices[skelID2];
int NDOF2 = c.bd2->getSkel()->getNumDofs();
Vector3d N12 = -c.normal;
int nDofs = c.bd2->getSkel()->getNumDofs();
MatrixXd J12( MatrixXd::Zero(3, nDofs) );
VectorXd invP = math::xformHom(c.bd2->getWorldInvTransform(), p);
t2.startTimer();
for(int dofIndex = 0; dofIndex < c.bd2->getNumDependentDofs(); dofIndex++) {
int index = c.bd2->getDependentDof(dofIndex);
VectorXd Jcol = math::xformHom(c.bd2->getDerivWorldTransform(dofIndex), (Vector3d)invP);
J12.col(index) = Jcol;
}
t2.stopTimer();
if (t2.getCount() == 1500)
t2.printScreen();
// MatrixXd J12 = getJacobian(c.bd2, p);
mN.block(index2, i, NDOF2, 1) = J12.transpose() * N12;
t1.stopTimer();
if (t1.getCount() == 1500) {
t1.printScreen();
cout << t2.getAve() / t1.getAve() * 100 << "%" << endl;
}
}
}
return;
}
*/
MatrixXd ContactDynamics::getTangentBasisMatrix(const Vector3d& p, const Vector3d& n) {
MatrixXd T(MatrixXd::Zero(3, mNumDir));
// Pick an arbitrary vector to take the cross product of (in this case, Z-axis)
Vector3d tangent = Vector3d::UnitZ().cross(n);
// If they're too close, pick another tangent (use X-axis as arbitrary vector)
if (tangent.norm() < EPSILON) {
tangent = Vector3d::UnitX().cross(n);
}
tangent.normalize();
// Rotate the tangent around the normal to compute bases.
// Note: a possible speedup is in place for mNumDir % 2 = 0
// Each basis and its opposite belong in the matrix, so we iterate half as many times.
double angle = (2 * M_PI) / mNumDir;
int iter = (mNumDir % 2 == 0) ? mNumDir / 2 : mNumDir;
for (int i = 0; i < iter; i++) {
Vector3d basis = Quaterniond(AngleAxisd(i * angle, n)) * tangent;
T.block<3,1>(0, i) = basis;
if (mNumDir % 2 == 0) {
T.block<3,1>(0, i + iter) = -basis;
}
}
return T;
}
Vector3d Needle::nearestPosition(const Vector3d& pos)
{
// Matrix<double, 3, 2> A;
// A.col(0) = rotation.col(2);
// A.col(1) = (rotation * AngleAxisd(-angle * M_PI/180.0, rotation.col(1))).col(2);
// Vector3d B = pos - position;
// Vector3d proj_B = A*(A.transpose()*A).inverse()*A.transpose()*B;
// double middle = (pos - position + radius * proj_B.normalized()).squaredNorm();
// double start = (pos - getStartPosition()).squaredNorm();
// double end = (pos - getEndPosition()).squaredNorm();
// if ((middle < start) && (middle < end)) {
// return position + radius * proj_B.normalized();
// } else {
// if (start < end)
// return getStartPosition();
// else
// return getEndPosition();
// }
double angle_per_link = angle/i_objs.size();
Vector3d min_pos = position + radius * rotation.col(2);
double min_dist = (min_pos - pos).squaredNorm();
for (int piece=1; piece<i_objs.size()+1; piece++) {
Vector3d candidate_pos = position + radius * (rotation * AngleAxisd(-angle_per_link * (piece) * M_PI/180.0, Vector3d::UnitY())).col(2);
double candidate_dist = (candidate_pos - pos).squaredNorm();
if (candidate_dist < min_dist) {
min_pos = candidate_pos;
min_dist = candidate_dist;
}
}
return min_pos;
}
void Needle::rotateAboutAxis(double degrees)
{
setTransform(position, AngleAxisd(degrees * M_PI/180.0, rotation.col(1)) * rotation);
if(isThreadAttached()) {
thread->updateConstrainedTransform(constraint_ind, getEndPosition(), getEndRotation());
}
}
Quaterniond quatFromRPY(double roll, double pitch, double yaw)
{
Quaterniond q ;
q = AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
return q ;
}
Matrix4d rotateZ(double _angle)
{
Matrix4d m = Matrix4d::Identity();
Matrix3d r;
r = AngleAxisd(_angle, Vector3d::UnitZ());
m.corner(TopLeft, 3, 3) = r;
return m;
}
void Camera::pitch(double angle)
{
Matrix3d T;
T=AngleAxisd(angle,_right);
_up=T*_up;
_look=T*_look;
}
inline void computeTempltOrientation(const Normal& n, double z_angle, Quaterniond& orientation)
{
Quaterniond rot_to_hm_frame;
rot_to_hm_frame.setFromTwoVectors(Vector3d(0, 0, 1), Vector3d(n.normal[0],
n.normal[1], n.normal[2]));
Quaterniond rot_about_z(AngleAxisd(z_angle, Vector3d(0, 0, 1)));
orientation = rot_to_hm_frame * rot_about_z;
}
void Camera::roll(double angle)
{
// only roll for aircraft type
if( _cameraType == AIRCRAFT )
{
Matrix3d T;
T=AngleAxisd(angle,_look);
// rotate _up and _right around _look vector
_right=T*_right;
_up=T*_up;
}
}
void PR2EihMapping::publish_home_pose() {
// publish home pose for check_handle_grasps
Matrix4d home_pose = arm->fk(home_joints);
Affine3d home_pose_affine = Translation3d(home_pose.block<3,1>(0,3)) * AngleAxisd(home_pose.block<3,3>(0,0));
geometry_msgs::PoseStamped home_pose_msg;
tf::Pose tf_pose;
tf::poseEigenToTF(home_pose_affine, tf_pose);
tf::poseTFToMsg(tf_pose, home_pose_msg.pose);
home_pose_msg.header.frame_id = "base_link";
home_pose_msg.header.stamp = ros::Time::now();
home_pose_pub.publish(home_pose_msg);
}
void PositionController::control(double t) {
// Evaluate trajectory
TrajectoryState s = this->getTargetState(t);
ModelState cur = vehicle->arrival_state();
Vector3d eP = s.position - cur.position;
Vector3d eV = s.velocity - cur.velocity;
// At low altitudes, there's no room to rotate so we reduce the error effect
// TODO: Have a better way of doing this: essentially we want a trajectory straight up followed
if(s.position.z() < 0.1) {
double factor = 0.8;
for(unsigned i = 0; i < 2; i++) {
eP(i) *= factor;
eV(i) *= factor;
}
}
Vector3d out;
if(hoverMode) {
// Do nothing if hovering on the ground
if(point.z() < 0.1) {
vehicle->setpoint_zero();
return;
}
out = hoverPid->compute(eP, eV, 0.01);
}
else {
out = pid->compute(eP, eV, 0.01 /* TODO: Make this more dynamic */);
}
Vector3d a = out + s.acceleration;
double yaw_angle = DEFAULT_YAW_ANGLE;
if(directAttitudeControl) {
vehicle->lastControlInput = a;
a += Vector3d(0, 0, GRAVITY_MS);
Quaterniond att = Quaterniond::FromTwoVectors(Vector3d(0,0,1), a.normalized());
Quaterniond yaw( AngleAxisd(yaw_angle, Vector3d::UnitZ()) );
vehicle->setpoint_attitude(att*yaw, a.norm());
}
else {
vehicle->setpoint_accel(a, yaw_angle);
}
}
void Camera::yaw(double angle)
{
Matrix3d T;
// rotate around world y (0, 1, 0) always for land object
//if( _cameraType == LANDOBJECT )
T=AngleAxisd(angle,Vector3d(0,1,0));
// rotate around own up vector for aircraft
// if( _cameraType == AIRCRAFT )
//T=AngleAxisd(angle,_up);
// rotate _right and _look around _up or y-axis
_right=T*_right;
_look=T*_look;
}
static void translate_shape(const Vector3d &translate, double modulation)
{
Vector3d mod;
/* rotate and scale shape depending on height along Z-axis */
Matrix3d m;
m = AngleAxisd(M_PI/(MARBLE_RUN_RESOLUTION/2),Vector3d::UnitZ());
/* calculate modulation vector */
mod = Vector3d(g_shape[0](0), g_shape[0](1), 0);
mod = mod.normalized()*modulation + translate;
/* apply scaling matrix to all shape points */
for(int i=0; i<SHAPE_POINTS; i++)
g_target[i] = (m * g_shape[i]) + mod;
}
void update_camera( float animation_delta_time, Vector2d &window_steering )
{
const unsigned leeway = 70, border = 50;
float degrees_per_frame = .02f * animation_delta_time;
float meters_per_frame = 10.f * animation_delta_time;
cout << animation_time << endl;
// Determine camera rotation movement first
Vector2f movement_plus ( window_steering[0] + leeway, window_steering[1] + leeway ); // movement[] is mouse position relative to canvas center; leeway is a tolerance from the center.
Vector2f movement_minus ( window_steering[0] - leeway, window_steering[1] - leeway );
bool outside_border = false;
for( int i = 0; i < 2; i++ )
if ( abs( window_steering[i] ) > g.get_window_size()[i]/2 - border ) outside_border = true; // Stop steering if we're on the outer edge of the canvas.
for( int i = 0; looking && outside_border == false && i < 2; i++ ) // Steer according to "movement" vector, but don't start increasing until outside a leeway window from the center.
{
float angular_velocity = ( ( movement_minus[i] > 0) * movement_minus[i] + ( movement_plus[i] < 0 ) * movement_plus[i] ) * degrees_per_frame; // Use movement's quantity conditionally
camera_transform = Affine3d( AngleAxisd( angular_velocity, Vector3d( i, 1-i, 0 ) ) ) * camera_transform; // On X step, rotate around Y axis, and vice versa.
}
camera_transform = Affine3d(Translation3d( meters_per_frame * thrust )) * camera_transform; // Now translation movement of camera, applied in local camera coordinate frame
}
Quaterniond lookAt(const Eigen::Vector3d &dir, double roll)
{
Vector3d nz = dir, na, nb ;
nz.normalize() ;
double q = sqrt(nz.x() * nz.x() + nz.y() * nz.y()) ;
if ( q < 1.0e-4 )
{
na = Vector3d(1, 0, 0) ;
nb = nz.cross(na) ;
}
else {
na = Vector3d(-nz.y()/q, nz.x()/q, 0) ;
nb = Vector3d(-nz.x() * nz.z()/q, -nz.y() * nz.z()/q, q) ;
}
Matrix3d r ;
r << na, nb, nz ;
return Quaterniond(r) * AngleAxisd(roll, Eigen::Vector3d::UnitZ()) ;
}
void World::applyRelativeControl(const vector<Control*>& controls, double thresh, bool limit_displacement)
{
assert(cursors.size() == controls.size());
for (int i = 0; i < cursors.size(); i++) {
Cursor* cursor = cursors[i];
Matrix3d rotate(controls[i]->getRotate());
AngleAxisd rotate_aa(rotate);
AngleAxisd noise_rot = AngleAxisd(normRand(0, thresh*rotate_aa.angle()) * M_PI/180.0, Vector3d(normRand(0, 1.0), normRand(0, 1.0), normRand(0, 1.0)).normalized());
const Matrix3d cursor_rot = cursor->rotation * rotate * noise_rot;
double trans_norm = controls[i]->getTranslate().norm();
const Vector3d noise_vec = Vector3d(normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm));
const Vector3d cursor_pos = cursor->position + controls[i]->getTranslate() + EndEffector::grab_offset * cursor_rot.col(0) + noise_vec;
cursor->setTransform(cursor_pos, cursor_rot, limit_displacement);
if (controls[i]->getButton(UP))
cursor->openClose(limit_displacement);
if (controls[i]->getButton(DOWN))
cursor->attachDettach(limit_displacement);
}
for (int thread_ind = 0; thread_ind < threads.size(); thread_ind++) {
threads[thread_ind]->minimize_energy();
}
vector<EndEffector*> end_effs;
getObjects<EndEffector>(end_effs);
for (int ee_ind = 0; ee_ind < end_effs.size(); ee_ind++) {
if (!(controls[0]->getButton(UP)) && !(controls[1]->getButton(UP)))
end_effs[ee_ind]->updateTransformFromAttachment();
}
}
vector<Marker> Visualization::visualizeGripper(const string& ns,
const string& frame_id, const geometry_msgs::Pose& pose, double gripper_state) const
{
Vector3d trans(pose.position.x, pose.position.y, pose.position.z);
Quaterniond rot(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
Marker l_finger = visualizeGripperPart("package://pr2_description/meshes/gripper_v0"
"/l_finger_tip.dae", ns, frame_id, 0);
Marker r_finger = visualizeGripperPart("package://pr2_description/meshes/gripper_v0"
"/l_finger_tip.dae", ns, frame_id, 1);
Marker palm;
palm = visualizeGripperPart("package://pr2_description/meshes/gripper_v0"
"/gripper_palm.dae", ns, frame_id, 2);
Marker bounding_box = visualizeGripperBox("bounding_box", frame_id, 4);
bounding_box.pose = pose;
Vector3d position(-0.051, 0, -0.025);
Quaterniond rotation = Quaterniond::Identity();
position = rot * position + trans;
rotation = rotation * rot;
palm.pose.position.x = position.x();
palm.pose.position.y = position.y();
palm.pose.position.z = position.z();
palm.pose.orientation.w = rotation.w();
palm.pose.orientation.x = rotation.x();
palm.pose.orientation.y = rotation.y();
palm.pose.orientation.z = rotation.z();
double finger_y_delta = gripper_state * 0.8 / 2;
double finger_x_delta = 0;
position = Vector3d(0.1175 - finger_x_delta, 0.015 + finger_y_delta, -0.025);
rotation = Quaterniond::Identity();
position = rot * position + trans;
rotation = rotation * rot;
l_finger.pose.position.x = position.x();
l_finger.pose.position.y = position.y();
l_finger.pose.position.z = position.z();
l_finger.pose.orientation.w = rotation.w();
l_finger.pose.orientation.x = rotation.x();
l_finger.pose.orientation.y = rotation.y();
l_finger.pose.orientation.z = rotation.z();
position = Vector3d(0.1175 - finger_x_delta, -0.015 - finger_y_delta, -0.025);
rotation = Quaterniond(AngleAxisd(M_PI, Vector3d(1, 0, 0)));
position = rot * position + trans;
rotation = rot * rotation;
r_finger.pose.position.x = position.x();
r_finger.pose.position.y = position.y();
r_finger.pose.position.z = position.z();
r_finger.pose.orientation.w = rotation.w();
r_finger.pose.orientation.x = rotation.x();
r_finger.pose.orientation.y = rotation.y();
r_finger.pose.orientation.z = rotation.z();
vector < Marker > result;
result.push_back(palm);
result.push_back(l_finger);
result.push_back(r_finger);
/* palm marker */
getTemplateExtractionPoint(position);
rotation = Quaterniond::Identity();
position = rot * position + trans;
rotation = rotation * rot;
Marker palm_marker;
palm_marker.header.frame_id = frame_id;
palm_marker.header.stamp = ros::Time::now();
palm_marker.ns = ns;
palm_marker.id = 3;
palm_marker.type = Marker::SPHERE;
palm_marker.action = Marker::ADD;
palm_marker.pose.position.x = position.x();
palm_marker.pose.position.y = position.y();
palm_marker.pose.position.z = position.z();
palm_marker.pose.orientation.x = 0.0;
palm_marker.pose.orientation.y = 0.0;
palm_marker.pose.orientation.z = 0.0;
palm_marker.pose.orientation.w = 1.0;
palm_marker.scale.x = 0.01;
palm_marker.scale.y = 0.01;
palm_marker.scale.z = 0.01;
palm_marker.color.a = 1.0;
palm_marker.color.r = 0.0;
palm_marker.color.g = 1.0;
result.push_back(palm_marker);
return result;
}
void World::initRestingThread(int opt)
{
int numInit = 5;
double first_length = 3.0; //FIRST_REST_LENGTH;
double second_length = SECOND_REST_LENGTH;
double middle_length = DEFAULT_REST_LENGTH;
vector<Vector3d> vertices;
vector<double> angles;
vector<double> lengths;
vector<Vector3d> directions;
if (opt == 0 || opt == 1) {
for (int i=0; i < 4*numInit + 5; i++)
angles.push_back(0.0);
lengths.push_back(first_length);
lengths.push_back(second_length);
for (int i=0; i < 4*numInit; i++)
lengths.push_back(middle_length);
lengths.push_back(second_length);
lengths.push_back(first_length);
lengths.push_back(first_length);
directions.push_back(Vector3d::UnitX());
directions.push_back(Vector3d::UnitX());
Vector3d direction = Vector3d(1.0, 1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit; i++)
directions.push_back(direction);
direction = Vector3d(1.0, -1.0, 0.4);
direction.normalize();
for (int i=0; i < numInit; i++)
directions.push_back(direction);
direction = Vector3d(-1.0, -1.0, -0.4);
direction.normalize();
for (int i=0; i < numInit; i++)
directions.push_back(direction);
direction = Vector3d(0.0, -1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit; i++)
directions.push_back(direction);
directions.push_back(-Vector3d::UnitY());
directions.push_back(-Vector3d::UnitY());
vertices.push_back(Vector3d::Zero());
for (int i=1; i < 4*numInit + 5; i++) {
Vector3d next_vertex;
next_vertex(0) = vertices[i-1](0) + directions[i-1](0) * lengths[i-1];
next_vertex(1) = vertices[i-1](1) + directions[i-1](1) * lengths[i-1];
next_vertex(2) = vertices[i-1](2) + directions[i-1](2) * lengths[i-1];
vertices.push_back(next_vertex);
}
Vector3d last_pos = Vector3d(-10.0, -30.0, 0.0);
for (int i=0; i<vertices.size(); i++)
vertices[i] += -vertices.back() + last_pos;
Matrix3d start_rotation0 = Matrix3d::Identity();
Matrix3d end_rotation0 = (Matrix3d) AngleAxisd(-M_PI/2.0, Vector3d::UnitZ());
ThreadConstrained* thread0 = new ThreadConstrained(vertices, angles, lengths, start_rotation0, end_rotation0, this);
threads.push_back(thread0);
}
if (opt == 0 || opt == 2) {
int numInit1 = 2;
angles.clear();
for (int i=0; i < 16*numInit1 + 5; i++)
angles.push_back(0.0);
lengths.clear();
lengths.push_back(first_length);
lengths.push_back(second_length);
for (int i=0; i < 16*numInit1; i++)
lengths.push_back(middle_length);
lengths.push_back(second_length);
lengths.push_back(first_length);
lengths.push_back(first_length);
directions.clear();
directions.push_back(-Vector3d::UnitX());
directions.push_back(-Vector3d::UnitX());
Vector3d direction = Vector3d(-1.0, 0.0, 1.0);
direction.normalize();
for (int i=0; i < 1.5*numInit1; i++)
directions.push_back(direction);
direction = Vector3d(0.0, -1.0, 2.0);
direction.normalize();
for (int i=0; i < 1.5*numInit1; i++)
directions.push_back(direction);
direction = Vector3d(-2.0, 1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit1; i++)
directions.push_back(direction);
direction = Vector3d(-2.0, -1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit1; i++)
directions.push_back(direction);
direction = Vector3d(0.0, 1.0, -2.0);
direction.normalize();
for (int i=0; i < 3*numInit1; i++)
directions.push_back(direction);
direction = Vector3d(0.0, -1.0, -2.0);
direction.normalize();
for (int i=0; i < 3*numInit1; i++)
directions.push_back(direction);
direction = Vector3d(2.0, 1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit1; i++)
directions.push_back(direction);
direction = Vector3d(2.0, -1.0, 0.0);
direction.normalize();
for (int i=0; i < numInit1; i++)
directions.push_back(direction);
direction = Vector3d(0.0, 1.0, 2.0);
direction.normalize();
for (int i=0; i < 1.5*numInit1; i++)
directions.push_back(direction);
direction = Vector3d(0.0, -1.0, 2.0);
direction.normalize();
for (int i=0; i < 1.5*numInit1; i++)
directions.push_back(direction);
directions.push_back(-Vector3d::UnitY());
directions.push_back(-Vector3d::UnitY());
vertices.clear();
vertices.push_back(Vector3d::Zero());
for (int i=1; i < 16*numInit1 + 5; i++) {
Vector3d next_vertex;
next_vertex(0) = vertices[i-1](0) + directions[i-1](0) * lengths[i-1];
next_vertex(1) = vertices[i-1](1) + directions[i-1](1) * lengths[i-1];
next_vertex(2) = vertices[i-1](2) + directions[i-1](2) * lengths[i-1];
vertices.push_back(next_vertex);
}
Vector3d last_pos = Vector3d(10.0, -30.0, 0.0);
for (int i=0; i<vertices.size(); i++)
vertices[i] += -vertices.back() + last_pos;
Matrix3d start_rotation1 = (Matrix3d) AngleAxisd(M_PI, Vector3d::UnitZ());
Matrix3d end_rotation1 = (Matrix3d) AngleAxisd(M_PI/2.0, Vector3d::UnitZ());
ThreadConstrained* thread1 = new ThreadConstrained(vertices, angles, lengths, start_rotation1, this);
threads.push_back(thread1);
}
for (int thread_ind=0; thread_ind < threads.size(); thread_ind++) {
#ifndef ISOTROPIC
Matrix2d B = Matrix2d::Zero();
B(0,0) = 10.0;
B(1,1) = 1.0;
threads[thread_ind]->set_coeffs_normalized(B, 3.0, 1e-4);
#else
threads[thread_ind]->set_coeffs_normalized(1.0, 3.0, 1e-4);
#endif
}
}
World::World(WorldManager* wm)
{
world_manager = wm;
if (world_manager != NULL)
collision_world = world_manager->allocateWorld(this);
else
collision_world = NULL;
//any of these pushes two threads into threads.
//initThread();
initThreadSingle();
//initLongerThread();
//initRestingThread(0);
//initRestingFinerThread(0);
//setting up control handles
cursors.push_back(new Cursor(Vector3d::Zero(), Matrix3d::Identity(), this, NULL));
cursors.push_back(new Cursor(Vector3d::Zero(), Matrix3d::Identity(), this, NULL));
//setting up objects in environment
//InfinitePlane* plane = new InfinitePlane(Vector3d(0.0, -30.0, 0.0), Vector3d(0.0, 1.0, 0.0), "../utils/textures/checkerBoardSquare32.bmp", this);
//InfinitePlane* plane = new InfinitePlane(Vector3d(0.0, -30.0, 0.0), Vector3d(0.0, 1.0, 0.0), 0.6, 0.6, 0.6, this);
//InfinitePlane* plane = new InfinitePlane(Vector3d(0.0, -30.0, 0.0), Vector3d(0.0, 1.0, 0.0), 0.42, 0.48, 0.55, this);
InfinitePlane* plane = new InfinitePlane(Vector3d(0.0, -30.0, 0.0), Vector3d(0.0, 1.0, 0.0), 139.0/255.0, 137.0/255.0, 137.0/255.0, this);
objs.push_back(plane);
//objs.push_back(new TexturedSphere(Vector3d::Zero(), 150.0, "../utils/textures/checkerBoardRect16.bmp", this));
// objs.push_back(new Box(plane->getPosition() + Vector3d(-40.0, 10.0, 0.0), Matrix3d::Identity(), Vector3d(10,10,10), 0.0, 0.5, 0.7, this));
//
// objs.push_back(new Needle(plane->getPosition() + Vector3d(0.0, 50.0, 0.0), Matrix3d::Identity(), 120.0, 10.0, 0.3, 0.3, 0.3, this));
// objs.push_back(new Needle(threads[0]->positionAtConstraint(0), threads[0]->rotationAtConstraint(0), 120.0, 10.0, 0.3, 0.3, 0.3, this, threads[0], 0));
//setting up end effectors
for (int i = 0; i < threads.size(); i++) {
objs.push_back(new EndEffector(threads[i]->positionAtConstraint(0), threads[i]->rotationAtConstraint(0), this, threads[i], 0));
assert((TYPE_CAST<EndEffector*>(objs.back()))->constraint_ind == 0);
objs.push_back(new EndEffector(threads[i]->positionAtConstraint(1), threads[i]->rotationAtConstraint(1), this, threads[i], threads[i]->numVertices()-1));
assert((TYPE_CAST<EndEffector*>(objs.back()))->constraint_ind == 1);
}
objs.push_back(new EndEffector(plane->getPosition() + Vector3d(30.0, EndEffector::short_handle_r, 0.0), (Matrix3d) AngleAxisd(-M_PI/2.0, Vector3d::UnitY()) * AngleAxisd(M_PI/2.0, Vector3d::UnitX()), this));
assert((TYPE_CAST<EndEffector*>(objs.back()))->constraint_ind == -1);
assert((TYPE_CAST<EndEffector*>(objs.back()))->constraint == -1);
objs.push_back(new EndEffector(plane->getPosition() + Vector3d(35.0, EndEffector::short_handle_r, 0.0), (Matrix3d) AngleAxisd(-M_PI/2.0, Vector3d::UnitY()) * AngleAxisd(M_PI/2.0, Vector3d::UnitX()), this));
assert((TYPE_CAST<EndEffector*>(objs.back()))->constraint_ind == -1);
assert((TYPE_CAST<EndEffector*>(objs.back()))->constraint == -1);
}
//This is hack to easily determine the displacement of each control
void World::applyRelativeControl(const vector<Control*>& controls, vector<double>& displacements, double thresh, bool limit_displacement)
{
assert(cursors.size() == controls.size());
vector<Vector3d> pre_positions;
if ((cursors.size() == 2) || (displacements.size() == 2)) {
pre_positions.resize(2);
for (int i = 0; i < cursors.size(); i++) {
assert(cursors[i]->isAttached());
if (cursors[i]->isAttached())
pre_positions[i] = cursors[i]->end_eff->getPosition();
}
} else {
for (int i = 0; i < displacements.size(); i++) {
displacements[i] = -1;
}
}
for (int i = 0; i < cursors.size(); i++) {
Cursor* cursor = cursors[i];
Matrix3d rotate(controls[i]->getRotate());
AngleAxisd rotate_aa(rotate);
AngleAxisd noise_rot = AngleAxisd(normRand(0, thresh*rotate_aa.angle()) * M_PI/180.0, Vector3d(normRand(0, 1.0), normRand(0, 1.0), normRand(0, 1.0)).normalized());
const Matrix3d cursor_rot = cursor->rotation * rotate * noise_rot;
double trans_norm = controls[i]->getTranslate().norm();
const Vector3d noise_vec = Vector3d(normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm));
const Vector3d cursor_pos = cursor->position + controls[i]->getTranslate() + EndEffector::grab_offset * cursor_rot.col(0) + noise_vec;
cursor->setTransform(cursor_pos, cursor_rot, limit_displacement);
if (controls[i]->getButton(UP))
cursor->openClose(limit_displacement);
if (controls[i]->getButton(DOWN))
cursor->attachDettach(limit_displacement);
}
for (int thread_ind = 0; thread_ind < threads.size(); thread_ind++) {
threads[thread_ind]->minimize_energy();
}
vector<EndEffector*> end_effs;
getObjects<EndEffector>(end_effs);
for (int ee_ind = 0; ee_ind < end_effs.size(); ee_ind++) {
if (!(controls[0]->getButton(UP)) && !(controls[1]->getButton(UP)))
end_effs[ee_ind]->updateTransformFromAttachment();
}
if ((cursors.size() == 2) || (displacements.size() == 3)) {
Matrix<double,6,1> u_tran;
for (int i = 0; i < cursors.size(); i++) {
assert(cursors[i]->isAttached());
if (cursors[i]->isAttached())
//cout << "norm of end effector " << i << ": " << (cursors[i]->end_eff->getPosition() - pre_positions[i]).norm() << endl;
displacements[i] = (cursors[i]->end_eff->getPosition() - pre_positions[i]).norm();
u_tran.segment(i*3,3) = (cursors[i]->end_eff->getPosition() - pre_positions[i]);
}
displacements[2] = u_tran.norm();
}
}
Matrix3d Needle::getEndRotation()
{
return AngleAxisd(-angle * M_PI/180.0, rotation.col(1)) * rotation;
}
enum TrajectoryFollower::FOLLOWER_STATUS TrajectoryFollower::traverseTrajectory(Eigen::Vector2d &motionCmd, const base::Pose &robotPose)
{
motionCmd(0) = 0.0;
motionCmd(1) = 0.0;
if(!hasTrajectory)
return REACHED_TRAJECTORY_END;
base::Trajectory &trajectory(currentTrajectory);
if(newTrajectory)
{
newTrajectory = false;
para = trajectory.spline.findOneClosestPoint(robotPose.position, 0.001);
}
pose.position = robotPose.position;
pose.heading = robotPose.getYaw();
if ( para < trajectory.spline.getEndParam() )
{
double dir = 1.0;
if(!trajectory.driveForward())
{
pose.heading = angleLimit(pose.heading+M_PI);
dir = -1.0;
}
double fwLenght = 0;
if(controllerType == 0)
{
fwLenght = dir * forwardLength + gpsCenterofRotationOffset;
}
else
{
//fwLenght = dir * gpsCenterofRotationOffset;
}
pose.position += AngleAxisd(pose.heading, Vector3d::UnitZ()) * Vector3d(fwLenght, 0, 0);
Eigen::Vector3d vError = trajectory.spline.poseError(pose.position, pose.heading, para);
para = vError(2);
//distance error
error.d = vError(0);
//heading error
error.theta_e = angleLimit(vError(1) + addPoseErrorY);
//spline parameter for traget point on spline
error.param = vError(2);
curvePoint.pose.position = trajectory.spline.getPoint(para);
curvePoint.pose.heading = trajectory.spline.getHeading(para);
curvePoint.param = para;
//disable this test for testing, as it seems to be not needed
bInitStable = true;
if(!bInitStable)
{
switch(controllerType)
{
case 0:
bInitStable = oTrajController_nO.checkInitialStability(error.d, error.theta_e, trajectory.spline.getCurvatureMax());
bInitStable = true;
break;
case 1:
bInitStable = oTrajController_P.checkInitialStability(error.d, error.theta_e, trajectory.spline.getCurvature(para), trajectory.spline.getCurvatureMax());
break;
case 2:
bInitStable = oTrajController_PI.checkInitialStability(error.d, error.theta_e, trajectory.spline.getCurvature(para), trajectory.spline.getCurvatureMax());
break;
default:
throw std::runtime_error("Got bad controllerType value");
}
if (!bInitStable)
{
LOG_DEBUG_S << "Trajectory controller: failed initial stability test";
return INITIAL_STABILITY_FAILED;
}
}
double vel = currentTrajectory.speed;
switch(controllerType)
{
case 0:
motionCmd = oTrajController_nO.update(vel, error.d, error.theta_e);
break;
case 1:
motionCmd = oTrajController_P.update(vel, error.d, error.theta_e, trajectory.spline.getCurvature(para), trajectory.spline.getVariationOfCurvature(para));
break;
case 2:
motionCmd = oTrajController_PI.update(vel, error.d, error.theta_e, trajectory.spline.getCurvature(para), trajectory.spline.getVariationOfCurvature(para));
break;
default:
throw std::runtime_error("Got bad controllerType value");
}
LOG_DEBUG_S << "Mc: " << motionCmd(0) << " " << motionCmd(1)
<< " error: d " << error.d << " theta " << error.theta_e << " PI";
}
else
{
if(status != REACHED_TRAJECTORY_END)
{
LOG_INFO_S << "Reached end of trajectory" << std::endl;
status = REACHED_TRAJECTORY_END;
}
return REACHED_TRAJECTORY_END;
}
if(status != RUNNING)
{
LOG_INFO_S << "Started to follow trajectory" << std::endl;
status = RUNNING;
}
return RUNNING;
}
void roll_right() { camera_transform *= Affine3d( AngleAxisd( 3 * PI /180, Vector3d( 0, 0, -1 ) ) ).matrix(); }
