chrono_robot_pose.h

Go to the documentation of this file.

#ifndef CHRONO_ROBOT_POSE_H
#define CHRONO_ROBOT_POSE_H
 
#include "bitrl/bitrl_config.h"
 
#ifdef BITRL_CHRONO
#include "bitrl/bitrl_types.h"
#include <chrono/physics/ChBody.h>
#include <string>
namespace bitrl
{
namespace rb::bitrl_chrono
{
 
class CHRONO_RobotPose
{
public:
 
    explicit CHRONO_RobotPose(std::shared_ptr<chrono::ChBody> robot_ptr=nullptr);
 
    void set_body(std::shared_ptr<chrono::ChBody> robot_ptr){robot_ptr_ = robot_ptr;}
 
    const chrono::ChVector3d& position() const {return robot_ptr_ -> GetPos();}
 
    const chrono::ChVector3d& velocity()const {return  robot_ptr_ -> GetLinVel();}
 
    const chrono::ChVector3d& acceleration()const {return  robot_ptr_ -> GetLinAcc();}
 
    const chrono::ChMatrix33d& rotation_matrix() const {return robot_ptr_ -> GetRotMat();}
 
    chrono::ChVector3d to_world_coords(chrono::ChVector3d point){return robot_ptr_ -> GetFrameRefToAbs().TransformPointLocalToParent(point);}
 
    chrono::ChVector3d to_local_coords(chrono::ChVector3d point){return robot_ptr_ -> GetFrameRefToAbs().TransformPointParentToLocal(point);}
private:
 
    std::shared_ptr<chrono::ChBody> robot_ptr_;
 
};
 
inline
CHRONO_RobotPose::CHRONO_RobotPose(std::shared_ptr<chrono::ChBody> robot_ptr)
    :
robot_ptr_(robot_ptr)
{}
}
}
 
#endif
 
#endif //CHRONO_ROBOT_STATE_H