<commit_before>// file: test_visual_elements_extractor.cpp
// This file links to treefksolverposfull_recursive.cpp thats provides a routine to solve forward kinematics 
// upon receipt of a joint_angles_t message.
// for the whole kinematic tree. KDL lib functions only provide an interface to query global position between
// a specified root and a tip segment.


#include <iostream>

#include <lcm/lcm-cpp.hpp>
#include "lcmtypes/drc_lcmtypes.hpp"

#include "RobotStateListener.hpp"

using namespace std;
using namespace boost;

namespace fk 
{
  //==================constructor / destructor
  
  /**Subscribes to Robot URDF Model and to MEAS_JOINT_ANGLES.*/
  RobotStateListener::RobotStateListener(boost::shared_ptr<lcm::LCM> &lcm, BotViewer *viewer):
    _urdf_parsed(false),
    _lcm(lcm),
    _viewer(viewer)

  {
    //lcm ok?
    if(!lcm->good())
      {
	cerr << "\nLCM Not Good: Joint Angles Handler" << endl;
	return;
      }
    
    // Subscribe to Robot_Model.  Will unsubscribe once a single message has been received
    _urdf_subscription = lcm->subscribe("ROBOT_MODEL", 
				       &fk::RobotStateListener::handleRobotUrdfMsg,
				       this);    
    //Subscribes to MEAS_JOINT_ANGLES 
    lcm->subscribe("MEAS_JOINT_ANGLES", &fk::RobotStateListener::handleJointAnglesMsg, this); //&this ?
  }
  


  RobotStateListener::~RobotStateListener() {}


  //=============message callbacks

  void RobotStateListener::handleJointAnglesMsg(const lcm::ReceiveBuffer* rbuf,
						 const string& chan, 
						 const drc::joint_angles_t* msg)
  {
    if (!_urdf_parsed)
      {
	cout << "\n handleJointAnglesMsg: Waiting for urdf to be parsed" << endl;
	return;
      }
    
    //clear stored data
    _link_tfs.clear();
    _link_shapes.clear();    

    // call a routine that calculates the transforms the joint_state_t* msg.
    map<string, double> jointpos_in;
    for (uint i=0; i< (uint) msg->num_joints; i++) //cast to uint to suppress compiler warning
      jointpos_in.insert(make_pair(msg->joint_name[i], msg->angular_position[i]));  
    
    map<string, drc::transform_t > cartpos_out;
    
		  // Calculate forward position kinematics
    bool kinematics_status;
    bool flatten_tree=true; // determines the absolute transforms with respect to robot origin. 
                            //Otherwise returns relative transforms between joints. 
    
    kinematics_status = _fksolver->JntToCart(jointpos_in,cartpos_out,flatten_tree);
    
    if(kinematics_status>=0){
      // cout << "Success!" <<endl;
    }else{
       cerr << "Error: could not calculate forward kinematics!" <<endl;
       return;
   }
    
    // PRINTS THE VISUAL PROPERTIES OF ALL LINKS THAT HAVE A VISUAL ELEMENT DEFINED IN THE URDF FILE 
    map<string, drc::transform_t>::const_iterator transform_it;
    typedef map<string, shared_ptr<urdf::Link> > links_mapType;
    for( links_mapType::const_iterator it =  _links_map.begin(); it!= _links_map.end(); it++)
      {  		

	if(it->second->visual)
	  {
	    
	    urdf::Pose visual_origin = it->second->visual->origin;
	    
	    KDL::Frame T_visualorigin_parentjoint, T_parentjoint_bodyorigin, T_visualorigin_bodyorigin;
	    
	    T_visualorigin_parentjoint.p[0]=visual_origin.position.x;
	    T_visualorigin_parentjoint.p[1]=visual_origin.position.y;
	    T_visualorigin_parentjoint.p[2]=visual_origin.position.z;
	    T_visualorigin_parentjoint.M.Quaternion(visual_origin.rotation.x,visual_origin.rotation.y,visual_origin.rotation.z,visual_origin.rotation.w);
	    
	    transform_it=cartpos_out.find(it->first);
	    
	    T_parentjoint_bodyorigin.p[0]= transform_it->second.translation.x;
	    T_parentjoint_bodyorigin.p[1]= transform_it->second.translation.y;
	    T_parentjoint_bodyorigin.p[2]= transform_it->second.translation.z;			
	    T_parentjoint_bodyorigin.M.Quaternion(transform_it->second.rotation.x,transform_it->second.rotation.y,transform_it->second.rotation.z,transform_it->second.rotation.w);
	    
	    T_visualorigin_bodyorigin = T_visualorigin_parentjoint*T_parentjoint_bodyorigin; 
	    
	    drc::link_transform_t state;	    
	    
	    state.link_name = transform_it->first;<commit_msg>Bug Fix with Quaternions<commit_after>// file: test_visual_elements_extractor.cpp
// This file links to treefksolverposfull_recursive.cpp thats provides a routine to solve forward kinematics 
// upon receipt of a joint_angles_t message.
// for the whole kinematic tree. KDL lib functions only provide an interface to query global position between
// a specified root and a tip segment.


#include <iostream>

#include <lcm/lcm-cpp.hpp>
#include "lcmtypes/drc_lcmtypes.hpp"

#include "RobotStateListener.hpp"

using namespace std;
using namespace boost;

namespace fk 
{
  //==================constructor / destructor
  
  /**Subscribes to Robot URDF Model and to MEAS_JOINT_ANGLES.*/
  RobotStateListener::RobotStateListener(boost::shared_ptr<lcm::LCM> &lcm, BotViewer *viewer):
    _urdf_parsed(false),
    _lcm(lcm),
    _viewer(viewer)

  {
    //lcm ok?
    if(!lcm->good())
      {
	cerr << "\nLCM Not Good: Joint Angles Handler" << endl;
	return;
      }
    
    // Subscribe to Robot_Model.  Will unsubscribe once a single message has been received
    _urdf_subscription = lcm->subscribe("ROBOT_MODEL", 
				       &fk::RobotStateListener::handleRobotUrdfMsg,
				       this);    
    //Subscribes to MEAS_JOINT_ANGLES 
    lcm->subscribe("MEAS_JOINT_ANGLES", &fk::RobotStateListener::handleJointAnglesMsg, this); //&this ?
  }
  


  RobotStateListener::~RobotStateListener() {}


  //=============message callbacks

  void RobotStateListener::handleJointAnglesMsg(const lcm::ReceiveBuffer* rbuf,
						 const string& chan, 
						 const drc::joint_angles_t* msg)
  {
    if (!_urdf_parsed)
      {
	cout << "\n handleJointAnglesMsg: Waiting for urdf to be parsed" << endl;
	return;
      }
    
    //clear stored data
    _link_tfs.clear();
    _link_shapes.clear();    

    // call a routine that calculates the transforms the joint_state_t* msg.
    map<string, double> jointpos_in;
    for (uint i=0; i< (uint) msg->num_joints; i++) //cast to uint to suppress compiler warning
      jointpos_in.insert(make_pair(msg->joint_name[i], msg->angular_position[i]));  
    
    map<string, drc::transform_t > cartpos_out;
    
		  // Calculate forward position kinematics
    bool kinematics_status;
    bool flatten_tree=true; // determines the absolute transforms with respect to robot origin. 
                            //Otherwise returns relative transforms between joints. 
    
    kinematics_status = _fksolver->JntToCart(jointpos_in,cartpos_out,flatten_tree);
    
    if(kinematics_status>=0){
      // cout << "Success!" <<endl;
    }else{
       cerr << "Error: could not calculate forward kinematics!" <<endl;
       return;
   }
    
    // PRINTS THE VISUAL PROPERTIES OF ALL LINKS THAT HAVE A VISUAL ELEMENT DEFINED IN THE URDF FILE 
    map<string, drc::transform_t>::const_iterator transform_it;
    typedef map<string, shared_ptr<urdf::Link> > links_mapType;
    for( links_mapType::const_iterator it =  _links_map.begin(); it!= _links_map.end(); it++)
      {  		

	if(it->second->visual)
	  {
	    
	    urdf::Pose visual_origin = it->second->visual->origin;
	    
	    KDL::Frame T_visualorigin_parentjoint, T_parentjoint_bodyorigin, T_visualorigin_bodyorigin;
	    
	    T_visualorigin_parentjoint.p[0]=visual_origin.position.x;
	    T_visualorigin_parentjoint.p[1]=visual_origin.position.y;
	    T_visualorigin_parentjoint.p[2]=visual_origin.position.z;

	    //T_visualorigin_parentjoint.M.Quaternion(visual_origin.rotation.x,visual_origin.rotation.y,visual_origin.rotation.z,visual_origin.rotation.w);
	    // EDIT: SISIR : The above code is wrong

    	    T_visualorigin_parentjoint.M =  KDL::Rotation::Quaternion(visual_origin.rotation.x, visual_origin.rotation.y, visual_origin.rotation.z, visual_origin.rotation.w);

	    transform_it=cartpos_out.find(it->first);
	    
	    T_parentjoint_bodyorigin.p[0]= transform_it->second.translation.x;
	    T_parentjoint_bodyorigin.p[1]= transform_it->second.translation.y;
	    T_parentjoint_bodyorigin.p[2]= transform_it->second.translation.z;	
		
	    //T_parentjoint_bodyorigin.M.Quaternion(transform_it->second.rotation.x,transform_it->second.rotation.y,transform_it->second.rotation.z,transform_it->second.rotation.w);
            // EDIT: SISIR : The above code is wrong
	    T_parentjoint_bodyorigin.M =  KDL::Rotation::Quaternion(transform_it->second.rotation.x, transform_it->second.rotation.y, transform_it->second.rotation.z, transform_it->second.rotation.w);

	    T_visualorigin_bodyorigin = T_visualorigin_parentjoint*T_parentjoint_bodyorigin; 
	    
	    drc::link_transform_t state;	    
	    
	    state.link_name = transform_it->first;
{'fraction_non_alphanumeric': 0.11271446426542438, 'fraction_numerical': 0.00181797523008749, 'mean_word_length': 3.6670201484623544, 'pattern_counts': {'":': 0, '<': 53, '<?xml version=': 0, '>': 67, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7251018', 'n_tokens_mistral': 3042, 'n_tokens_neox': 2824, 'n_words': 572}