Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Read floating base groundtruth #7

Merged
merged 3 commits into from
Jul 10, 2020
Merged

Read floating base groundtruth #7

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5ee1588
Connect floatingbase groundtruth entries
arntanguy Jul 10, 2020
dddbaff
Adapt to spdlog
arntanguy Jul 10, 2020
b886697
Ensure initialization of eigen types
arntanguy Jul 10, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
60 changes: 54 additions & 6 deletions src/MCControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ namespace
{
if(mc_rtc::MC_RTC_VERSION != mc_rtc::version())
{
LOG_ERROR("MCControl was compiled with " << mc_rtc::MC_RTC_VERSION << " but mc_rtc is at version " << mc_rtc::version() << ", you might face subtle issues or unexpected crashes, please recompile mc_openrtm")
mc_rtc::log::error("MCControl was compiled with {} but mc_rtc is at version {}, you might face subtle issues or unexpected crashes, please recompile mc_openrtm", mc_rtc::MC_RTC_VERSION, mc_rtc::version());
}
return false;
}
Expand All @@ -78,6 +78,9 @@ MCControl::MCControl(RTC::Manager* manager)
m_poseInIn("poseIn", m_poseIn),
m_velInIn("velIn", m_velIn),
m_taucInIn("taucIn", m_taucIn),
m_basePoseInIn("basePoseIn", m_basePoseIn),
m_baseVelInIn("baseVelIn", m_baseVelIn),
m_baseAccInIn("baseAccIn", m_baseAccIn),
m_wrenchesNames(),
m_qOutOut("qOut", m_qOut),
m_pOutOut("pOut", m_pOut),
Expand Down Expand Up @@ -108,7 +111,7 @@ MCControl::~MCControl() {}

RTC::ReturnCode_t MCControl::onInitialize()
{
LOG_INFO("MCControl::onInitialize() starting")
mc_rtc::log::info("MCControl::onInitialize() starting");
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
Expand All @@ -121,6 +124,12 @@ RTC::ReturnCode_t MCControl::onInitialize()
addInPort("poseIn", m_poseInIn);
addInPort("velIn", m_velInIn);
addInPort("taucIn", m_taucInIn);
// Floating base
addInPort("basePoseIn", m_basePoseInIn);
addInPort("baseVelIn", m_baseVelInIn);
addInPort("baseAccIn", m_baseAccInIn);

// Force sensors
for(size_t i = 0; i < m_wrenchesNames.size(); ++i)
{
addInPort(m_wrenchesNames[i].c_str(), *(m_wrenchesInIn[i]));
Expand All @@ -147,19 +156,19 @@ RTC::ReturnCode_t MCControl::onInitialize()
bindParameter("is_enabled", controller.running, "0");

// </rtc-template>
LOG_INFO("MCControl::onInitialize() finished")
mc_rtc::log::info("MCControl::onInitialize() finished");
return RTC::RTC_OK;
}

RTC::ReturnCode_t MCControl::onActivated(RTC::UniqueId ec_id)
{
LOG_INFO("onActivated")
mc_rtc::log::info("onActivated");
return RTC::RTC_OK;
}

RTC::ReturnCode_t MCControl::onDeactivated(RTC::UniqueId ec_id)
{
LOG_INFO("onDeactivated")
mc_rtc::log::info("onDeactivated");
return RTC::RTC_OK;
}

Expand Down Expand Up @@ -229,6 +238,36 @@ RTC::ReturnCode_t MCControl::onExecute(RTC::UniqueId ec_id)
accIn(1) = m_accIn.data.ay;
accIn(2) = m_accIn.data.az;
}
if(m_basePoseInIn.isNew())
{
m_basePoseInIn.read();
floatingBasePosIn.x() = m_basePoseIn.data.position.x;
floatingBasePosIn.y() = m_basePoseIn.data.position.y;
floatingBasePosIn.z() = m_basePoseIn.data.position.z;
floatingBaseRPYIn.x() = m_basePoseIn.data.orientation.r;
floatingBaseRPYIn.y() = m_basePoseIn.data.orientation.p;
floatingBaseRPYIn.z() = m_basePoseIn.data.orientation.y;
}
if(m_baseVelInIn.isNew())
{
m_baseVelInIn.read();
floatingBaseVelIn.angular().x() = m_baseVelIn.data[3];
floatingBaseVelIn.angular().y() = m_baseVelIn.data[4];
floatingBaseVelIn.angular().z() = m_baseVelIn.data[5];
floatingBaseVelIn.linear().x() = m_baseVelIn.data[0];
floatingBaseVelIn.linear().y() = m_baseVelIn.data[1];
floatingBaseVelIn.linear().z() = m_baseVelIn.data[2];
}
if(m_baseAccInIn.isNew())
{
m_baseAccInIn.read();
floatingBaseAccIn.angular().x() = m_baseAccIn.data[3];
floatingBaseAccIn.angular().y() = m_baseAccIn.data[4];
floatingBaseAccIn.angular().z() = m_baseAccIn.data[5];
floatingBaseAccIn.linear().x() = m_baseAccIn.data[0];
floatingBaseAccIn.linear().y() = m_baseAccIn.data[1];
floatingBaseAccIn.linear().z() = m_baseAccIn.data[2];
}
if(m_taucInIn.isNew())
{
m_taucInIn.read();
Expand Down Expand Up @@ -267,12 +306,21 @@ RTC::ReturnCode_t MCControl::onExecute(RTC::UniqueId ec_id)
controller.setEncoderValues(qIn);
controller.setWrenches(m_wrenches);
controller.setJointTorques(taucIn);

// Floating base sensor
controller.setSensorPositions(controller.robot(), {{"FloatingBase", floatingBasePosIn}});
controller.setSensorOrientations(controller.robot(),
{{"FloatingBase", Eigen::Quaterniond(mc_rbdyn::rpyToMat(floatingBaseRPYIn))}});
controller.setSensorAngularVelocities(controller.robot(), {{"FloatingBase", floatingBaseVelIn.angular()}});
controller.setSensorLinearVelocities(controller.robot(), {{"FloatingBase", floatingBaseVelIn.linear()}});
controller.setSensorAccelerations(controller.robot(), {{"FloatingBase", floatingBaseAccIn.linear()}});

if(controller.running)
{
double t = tm.sec * 1e9 + tm.nsec;
if(!init)
{
LOG_INFO("Init controller")
mc_rtc::log::info("Init controller");
auto q = Eigen::Quaterniond(mc_rbdyn::rpyToMat(rpyIn)).normalized();
controller.init(qIn, std::array<double, 7>{{q.w(), q.x(), q.y(), q.z(), pIn.x(), pIn.y(), pIn.z()}});
init = true;
Expand Down
20 changes: 16 additions & 4 deletions src/MCControl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,14 +113,14 @@ class MCControl : public RTC::DataFlowComponentBase
InPort<TimedOrientation3D> m_rpyInIn;
TimedAngularVelocity3D m_rateIn;
InPort<TimedAngularVelocity3D> m_rateInIn;
Eigen::Vector3d rateIn;
Eigen::Vector3d rateIn = Eigen::Vector3d::Zero();
TimedAcceleration3D m_accIn;
InPort<TimedAcceleration3D> m_accInIn;
Eigen::Vector3d accIn;
Eigen::Vector3d accIn = Eigen::Vector3d::Zero();
TimedPose3D m_poseIn;
InPort<TimedPose3D> m_poseInIn;
Eigen::Vector3d rpyIn;
Eigen::Vector3d pIn;
Eigen::Vector3d rpyIn = Eigen::Vector3d::Zero();
Eigen::Vector3d pIn = Eigen::Vector3d::Zero();
/* Velocity of the free flyer, given by simulator */
InPort<TimedDoubleSeq> m_velInIn;
TimedDoubleSeq m_velIn;
Expand All @@ -129,6 +129,18 @@ class MCControl : public RTC::DataFlowComponentBase
InPort<TimedDoubleSeq> m_taucInIn;
std::vector<double> taucIn;

// Floating base input (e.g simulation groundtruth)
RTC::InPort<RTC::TimedPose3D> m_basePoseInIn;
RTC::InPort<RTC::TimedDoubleSeq> m_baseVelInIn;
RTC::InPort<RTC::TimedDoubleSeq> m_baseAccInIn;
RTC::TimedPose3D m_basePoseIn;
RTC::TimedDoubleSeq m_baseVelIn;
RTC::TimedDoubleSeq m_baseAccIn;
Eigen::Vector3d floatingBasePosIn = Eigen::Vector3d::Zero();
Eigen::Vector3d floatingBaseRPYIn = Eigen::Vector3d::Zero();
sva::MotionVecd floatingBaseVelIn = sva::MotionVecd::Zero();
sva::MotionVecd floatingBaseAccIn = sva::MotionVecd::Zero();

std::vector<std::string> m_wrenchesNames;
std::vector<TimedDoubleSeq *> m_wrenchesIn;
std::vector<InPort<TimedDoubleSeq> *> m_wrenchesInIn;
Expand Down
2 changes: 1 addition & 1 deletion src/MCControlServiceSVC_impl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ class MCControlServiceSVC_impl : public virtual POA_OpenHRP::MCControlService,
MCControlServiceSVC_impl(MCControl * plugin);
virtual ~MCControlServiceSVC_impl();

virtual CORBA::Boolean EnableController(const char * name);
virtual CORBA::Boolean EnableController(const char * name) override;

/* Grippers (always available) */
virtual CORBA::Boolean open_grippers() override;
Expand Down