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);
}
