IFloatBuffer* Mark::getVertices(const Planet* planet) { if (_vertices == NULL) { const Vector3D* pos = getCartesianPosition(planet); FloatBufferBuilderFromCartesian3D vertex(CenterStrategy::noCenter(), Vector3D::zero()); vertex.add(*pos); vertex.add(*pos); vertex.add(*pos); vertex.add(*pos); _vertices = vertex.create(); } return _vertices; }
void Mark::render(const G3MRenderContext* rc, const Vector3D& cameraPosition) { const Planet* planet = rc->getPlanet(); const Vector3D* markPosition = getCartesianPosition(planet); const Vector3D markCameraVector = markPosition->sub(cameraPosition); // mark will be renderered only if is renderable by distance and placed on a visible globe area bool renderableByDistance; if (_minDistanceToCamera == 0) { renderableByDistance = true; } else { const double squaredDistanceToCamera = markCameraVector.squaredLength(); renderableByDistance = ( squaredDistanceToCamera <= (_minDistanceToCamera * _minDistanceToCamera) ); } _renderedMark = false; if (renderableByDistance) { const Vector3D normalAtMarkPosition = planet->geodeticSurfaceNormal(*markPosition); if (normalAtMarkPosition.angleBetween(markCameraVector)._radians > IMathUtils::instance()->halfPi()) { if (_textureId == NULL) { if (_textureImage != NULL) { _textureId = rc->getTexturesHandler()->getGLTextureId(_textureImage, GLFormat::rgba(), _imageID, false); rc->getFactory()->deleteImage(_textureImage); _textureImage = NULL; } } if (_textureId != NULL) { GL* gl = rc->getGL(); gl->drawBillBoard(_textureId, getVertices(planet), _textureWidth, _textureHeight); _renderedMark = true; } } } }
/*! * \brief Sets the finger positions */ void JacoComm::setFingerPositions(const FingerAngles &fingers, int timeout, bool push) { boost::recursive_mutex::scoped_lock lock(api_mutex_); if (isStopped()) { ROS_INFO("The fingers could not be set because the arm is stopped"); return; } int result = NO_ERROR_KINOVA; TrajectoryPoint jaco_position; jaco_position.InitStruct(); memset(&jaco_position, 0, sizeof(jaco_position)); // zero structure if (push) { result = jaco_api_.eraseAllTrajectories(); if (result != NO_ERROR_KINOVA) { throw JacoCommException("Could not erase trajectories", result); } } //startAPI(); result = jaco_api_.setAngularControl(); if (result != NO_ERROR_KINOVA) { throw JacoCommException("Could not set Cartesian control", result); } // Initialize Cartesian control of the fingers jaco_position.Position.HandMode = POSITION_MODE; jaco_position.Position.Type = ANGULAR_POSITION; jaco_position.Position.Fingers = fingers; jaco_position.Position.Delay = 0.0; jaco_position.LimitationsActive = 0; AngularPosition jaco_angles; memset(&jaco_angles, 0, sizeof(jaco_angles)); // zero structure result = jaco_api_.getAngularPosition(jaco_angles); if (result != NO_ERROR_KINOVA) { throw JacoCommException("Could not get the angular position", result); } jaco_position.Position.Actuators = jaco_angles.Actuators; // When loading a cartesian position for the fingers, values are required for the arm joints // as well or the arm goes nuts. Grab the current position and feed it back to the arm. JacoPose pose; getCartesianPosition(pose); jaco_position.Position.CartesianPosition = pose; result = jaco_api_.sendAdvanceTrajectory(jaco_position); if (result != NO_ERROR_KINOVA) { throw JacoCommException("Could not send advanced finger trajectory", result); } }
const Geodetic3D getGeodeticPosition() const { if (_geodeticPosition == NULL) { _geodeticPosition = new Geodetic3D( _planet->toGeodetic3D(getCartesianPosition()) ); } return *_geodeticPosition; }
void JtATIController::updateHook() { if(use_ft_sensor && port_ftdata.read(wrench_msg) != RTT::NewData) return; if(!updateState()) return; jnt_trq_cmd.setZero(); if(0) { getMassMatrix(mass); id_dyn_solver->JntToMass(jnt_pos_kdl,mass_kdl); RTT::log(RTT::Warning) << "mass : \n"<<mass<<RTT::endlog(); RTT::log(RTT::Warning) << "mass_kdl : \n"<<mass_kdl.data<<RTT::endlog(); } if(use_ft_sensor){ if(use_kdl){ // remove B(q,qdot) jnt_vel_kdl.data.setZero(); // Get the Wrench in the last segment's frame tf::wrenchMsgToKDL(wrench_msg.wrench,wrench_kdl); f_ext_kdl.back() = wrench_kdl; // Get The full dynamics with external forces id_rne_solver->CartToJnt(jnt_pos_kdl,jnt_vel_kdl,jnt_acc_kdl,f_ext_kdl,jnt_trq_kdl); // Get Joint Gravity torque id_dyn_solver->JntToGravity(jnt_pos_kdl,gravity_kdl); // remove G(q) jnt_trq_cmd = jnt_trq_kdl.data - gravity_kdl.data; RTT::log(RTT::Debug) << "Adding FT data : \n"<<jnt_trq_cmd.transpose()<<RTT::endlog(); }else{ // Get Jacobian with KRC getJacobian(J_tip_base); getCartesianPosition(cart_pos); tf::poseMsgToKDL(cart_pos,tool_in_base_frame); tf::poseMsgToEigen(cart_pos,tool_in_base_frame_eigen); J_tip_base.changeBase(tool_in_base_frame.M); RTT::log(RTT::Debug) << "J : \n"<<J_tip_base.data<<RTT::endlog(); // Get Jacobian with KDL jnt_to_jac_solver->JntToJac(jnt_pos_kdl,J_kdl,kdl_chain.getNrOfSegments()); RTT::log(RTT::Debug) << "J_kdl : \n"<<J_kdl.data<<RTT::endlog(); tf::wrenchMsgToEigen(wrench_msg.wrench,wrench); //tf::wrenchMsgToKDL(wrench_msg.wrench,wrench_kdl); wrench_kdl = tool_in_base_frame.M * wrench_kdl; ///tf::wrenchKDLToEigen(wrench_kdl,wrench); jnt_trq_cmd = J_tip_base.data.transpose() * wrench; } } if(use_kdl_gravity) { getGravityTorque(jnt_grav); id_dyn_solver->JntToGravity(jnt_pos_kdl,gravity_kdl); jnt_trq_cmd += kg.asDiagonal() * gravity_kdl.data - jnt_grav; RTT::log(RTT::Debug) << "Adding gravity : \n"<<(kg.asDiagonal() * gravity_kdl.data - jnt_grav).transpose()<<RTT::endlog(); } if(compensate_coriolis) { id_dyn_solver->JntToCoriolis(jnt_pos_kdl,jnt_vel_kdl,coriolis_kdl); jnt_trq_cmd -= coriolis_kdl.data.asDiagonal()*jnt_vel; RTT::log(RTT::Debug) << "Removing Coriolis : \n"<<(coriolis_kdl.data.asDiagonal()*jnt_vel).transpose()<<RTT::endlog(); } if(add_damping) { jnt_trq_cmd -= kd.asDiagonal() * jnt_vel; RTT::log(RTT::Debug) << "Adding damping : \n"<<-(kd.asDiagonal() * jnt_vel).transpose()<<RTT::endlog(); } RTT::log(RTT::Debug) << "jnt_trq_cmd : \n"<<jnt_trq_cmd.transpose()<<RTT::endlog(); sendJointTorque(jnt_trq_cmd); }