vpRobotWireFrameSimulator.cpp

/****************************************************************************
 *
 * $Id: vpRobotWireFrameSimulator.cpp 3530 2012-01-03 10:52:12Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
 * 
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional 
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Basic class used to make robot simulators.
 *
 * Authors:
 * Nicolas Melchior
 *
 *****************************************************************************/

#include <visp/vpConfig.h>



#if defined(_WIN32) || defined(VISP_HAVE_PTHREAD)
#include <visp/vpRobotWireFrameSimulator.h>
#include <visp/vpSimulatorViper850.h>

vpRobotWireFrameSimulator::vpRobotWireFrameSimulator()
  : vpWireFrameSimulator(), vpRobotSimulator(),
    I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL), artCoord(), artVel(), velocity(),
    /* thread(), attr(), */ mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(),
    displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
    cameraParam(),
#if defined(VISP_HAVE_DISPLAY)
    display(),
#endif
    displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false),
    setVelocityCalled(false), verbose_(false)
{
  setSamplingTime(0.010);
  velocity.resize(6);
  I.resize(480,640);
  I = 255;
#if defined(VISP_HAVE_DISPLAY)
  display.init(I, 0, 0,"The External view");
#endif

 //pid_t pid = getpid();
 // setpriority (PRIO_PROCESS, pid, 19);
}

vpRobotWireFrameSimulator::vpRobotWireFrameSimulator(bool do_display)
  : vpWireFrameSimulator(), vpRobotSimulator(),
    I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL), artCoord(), artVel(), velocity(),
    /* thread(), attr(), */ mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(),
    displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
    cameraParam(),
#if defined(VISP_HAVE_DISPLAY)
    display(),
#endif
    displayType(MODEL_3D), displayAllowed(do_display), constantSamplingTimeMode(false),
    setVelocityCalled(false), verbose_(false)
{
  setSamplingTime(0.010);
  velocity.resize(6);
  I.resize(480,640);
  I = 255;
  
#if defined(VISP_HAVE_DISPLAY)
  if (do_display)
    this->display.init(I, 0, 0,"The External view");
#endif  
   
 //pid_t pid = getpid();
 // setpriority (PRIO_PROCESS, pid, 19);
}



vpRobotWireFrameSimulator::~vpRobotWireFrameSimulator()
{
}

void
vpRobotWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object)
{
  if(displayCamera){
    free_Bound_scene (&(this->camera));
  }
  vpWireFrameSimulator::initScene(obj, desired_object);
  if(displayCamera){
    free_Bound_scene (&(this->camera));
  }
  displayCamera = false;
}

void
vpRobotWireFrameSimulator::initScene(const char* obj, const char* desired_object)
{
  if(displayCamera){
    free_Bound_scene (&(this->camera));
  }
  vpWireFrameSimulator::initScene(obj, desired_object);
  if(displayCamera){
    free_Bound_scene (&(this->camera));
  }
  displayCamera = false;
}

void
vpRobotWireFrameSimulator::initScene(const vpSceneObject &obj)
{
  if(displayCamera){
    free_Bound_scene (&(this->camera));
  }
  vpWireFrameSimulator::initScene(obj);
  if(displayCamera){
    free_Bound_scene (&(this->camera));
  }
  displayCamera = false;
}

void
vpRobotWireFrameSimulator::initScene(const char* obj)
{
  if(displayCamera){
    free_Bound_scene (&(this->camera));
  }
  vpWireFrameSimulator::initScene(obj);
  if(displayCamera){
    free_Bound_scene (&(this->camera));
  }
  displayCamera = false;
}

void
vpRobotWireFrameSimulator::getInternalView(vpImage<vpRGBa> &I_)
{
  
  if (!sceneInitialized)
    throw;

  double u;
  double v;
  //if(px_int != 1 && py_int != 1)
  // we assume px_int and py_int > 0
  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon()) 
      && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
  {
    u = (double)I_.getWidth()/(2*px_int);
    v = (double)I_.getHeight()/(2*py_int);
  }
  else
  {
    u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
    v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
  }

  float o44c[4][4],o44cd[4][4],x,y,z;
  Matrix id = IDENTITY_MATRIX;

  vpHomogeneousMatrix* fMit = new vpHomogeneousMatrix[size_fMi];
  get_fMi(fMit);
  this->cMo = fMit[size_fMi-1].inverse()*fMo;
  this->cMo = rotz*cMo;
  
  vp2jlc_matrix(cMo.inverse(),o44c);
  vp2jlc_matrix(cdMo.inverse(),o44cd);
  
  while (get_displayBusy()) vpTime::wait(2);

  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
  x = o44c[2][0] + o44c[3][0];
  y = o44c[2][1] + o44c[3][1];
  z = o44c[2][2] + o44c[3][2];
  add_vwstack ("start","vrp", x,y,z);
  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
  add_vwstack ("start","window", -u, u, -v, v);
  if (displayObject)
    display_scene(id,this->scene,I_, curColor);

  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
  x = o44cd[2][0] + o44cd[3][0];
  y = o44cd[2][1] + o44cd[3][1];
  z = o44cd[2][2] + o44cd[3][2];
  add_vwstack ("start","vrp", x,y,z);
  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
  add_vwstack ("start","window", -u, u, -v, v);
  if (displayDesiredObject)
  {
    if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I_, vpColor::red);
    else display_scene(id,desiredScene,I_, desColor);
  }
  delete[] fMit;
  set_displayBusy(false);
}

void
vpRobotWireFrameSimulator::getInternalView(vpImage<unsigned char> &I_)
{
  
  if (!sceneInitialized)
    throw;

  double u;
  double v;
  //if(px_int != 1 && py_int != 1)
  // we assume px_int and py_int > 0
  if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon()) 
      && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
  {
    u = (double)I.getWidth()/(2*px_int);
    v = (double)I.getHeight()/(2*py_int);
  }
  else
  {
    u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
    v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
  }

  float o44c[4][4],o44cd[4][4],x,y,z;
  Matrix id = IDENTITY_MATRIX;

  vpHomogeneousMatrix* fMit = new vpHomogeneousMatrix[size_fMi];
  get_fMi(fMit);
  this->cMo = fMit[size_fMi-1].inverse()*fMo;
  this->cMo = rotz*cMo;
  
  vp2jlc_matrix(cMo.inverse(),o44c);
  vp2jlc_matrix(cdMo.inverse(),o44cd);
  
  while (get_displayBusy()) vpTime::wait(2);

  add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
  x = o44c[2][0] + o44c[3][0];
  y = o44c[2][1] + o44c[3][1];
  z = o44c[2][2] + o44c[3][2];
  add_vwstack ("start","vrp", x,y,z);
  add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
  add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
  add_vwstack ("start","window", -u, u, -v, v);
  if (displayObject)
  {
    display_scene(id,this->scene,I_, curColor);
  }

  add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
  x = o44cd[2][0] + o44cd[3][0];
  y = o44cd[2][1] + o44cd[3][1];
  z = o44cd[2][2] + o44cd[3][2];
  add_vwstack ("start","vrp", x,y,z);
  add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
  add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
  add_vwstack ("start","window", -u, u, -v, v);
  if (displayDesiredObject)
  {
    if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I_, vpColor::red);
    else display_scene(id,desiredScene,I_, desColor);
  }
  delete[] fMit;
  set_displayBusy(false);
}

vpHomogeneousMatrix
vpRobotWireFrameSimulator::get_cMo()
{
  vpHomogeneousMatrix cMoTemp;
  vpHomogeneousMatrix* fMit = new vpHomogeneousMatrix[size_fMi];
  get_fMi(fMit);
  cMoTemp = fMit[size_fMi-1].inverse()*fMo;
  delete[] fMit;
  return cMoTemp;
}

#endif