Skip to content

Commit

Permalink
Merge pull request #741 from robotology/fix/740
Browse files Browse the repository at this point in the history 
Fix `ARE` check on action type
  • Loading branch information
@pattacini
pattacini authored Apr 8, 2021
2 parents 89971cc + 7ebc1d6 commit 0bf92fe
Showing 1 changed file with 43 additions and 44 deletions.
87 changes: 43 additions & 44 deletions src/modules/actionsRenderingEngine/src/MotorThread.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@
*/


#include <typeinfo>
#include <fstream>
#include <sstream>
#include <cmath>
Expand Down Expand Up @@ -1260,8 +1259,8 @@ bool MotorThread::threadInit()
action[arm]->getGraspModel(graspModel[arm]);
action[arm]->enableReachingTimeout(reachingTimeout);

if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->setExtForceThres(extForceThresh[arm]);
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->setExtForceThres(extForceThresh[arm]);

deque<string> q=action[arm]->getHandSeqList();
yInfo("***** List of available %s hand sequence keys:",arm_name[arm].c_str());
Expand Down Expand Up @@ -1426,8 +1425,8 @@ void MotorThread::run()

// get the wrench at the end-effector
// and filter out forces under threshold
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[dragger.arm])->getExtWrench(wrench);
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[dragger.arm]))
p->getExtWrench(wrench);

force[0]=wrench[0];
force[1]=wrench[1];
Expand Down Expand Up @@ -1469,8 +1468,8 @@ void MotorThread::run()

// get the wrench at the end-effector
// and filter out forces under threshold
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[dragger.arm])->getExtWrench(wrench);
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[dragger.arm]))
p->getExtWrench(wrench);

force[0]=wrench[0];
force[1]=wrench[1];
Expand Down Expand Up @@ -1627,8 +1626,8 @@ bool MotorThread::reach(Bottle &options)
restoreContext(arm);

wbdRecalibration();
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->enableContactDetection();
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->enableContactDetection();

ActionPrimitivesWayPoint wp;
wp.x=xd+tmpDisp; wp.o=tmpOrient; wp.oEnabled=true;
Expand All @@ -1645,10 +1644,10 @@ bool MotorThread::reach(Bottle &options)

bool f;
action[arm]->checkActionsDone(f,true);
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
{
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->checkContact(f);
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->disableContactDetection();
p->checkContact(f);
p->disableContactDetection();
}

action[arm]->enableReachingTimeout(reachingTimeout);
Expand Down Expand Up @@ -1725,17 +1724,17 @@ bool MotorThread::powerGrasp(Bottle &options)
restoreContext(arm);

wbdRecalibration();
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->enableContactDetection();
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->enableContactDetection();
action[arm]->enableReachingTimeout(0.6*reachingTimeout);
action[arm]->pushAction(approach_x,approach_o,"pregrasp_hand");

bool f;
action[arm]->pushAction(x,o);
action[arm]->checkActionsDone(f,true);
action[arm]->enableReachingTimeout(reachingTimeout);
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->disableContactDetection();
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->disableContactDetection();

return grasp(options);
}
Expand Down Expand Up @@ -1780,12 +1779,12 @@ bool MotorThread::push(Bottle &options)
}

wbdRecalibration();
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->enableContactDetection();
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->enableContactDetection();
action[arm]->pushAction(xd-3*push_direction*reachSideDisp[arm],reachSideOrient[arm]);
action[arm]->checkActionsDone(f,true);
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->disableContactDetection();
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->disableContactDetection();

if (!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
setGazeIdle();
Expand Down Expand Up @@ -1961,17 +1960,17 @@ bool MotorThread::takeTool(Bottle &options)
action[arm]->checkActionsDone(f,true);

double force_thresh;
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->getExtForceThres(force_thresh);
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->getExtForceThres(force_thresh);

bool contact_detected=false;
Vector wrench(6);
double t=Time::now();

while (!contact_detected && (Time::now()-t<5.0))
{
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->getExtWrench(wrench);
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->getExtWrench(wrench);

if (norm(wrench)>force_thresh)
contact_detected=true;
Expand Down Expand Up @@ -2007,16 +2006,16 @@ bool MotorThread::expect(Bottle &options)
action[arm]->checkActionsDone(f,true);

double force_thresh;
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->getExtForceThres(force_thresh);
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->getExtForceThres(force_thresh);

bool contact_detected=false;
Vector wrench(6);
double t=Time::now();
while(!contact_detected && Time::now()-t<5.0)
{
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->getExtWrench(wrench);
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->getExtWrench(wrench);

if(norm(wrench)>force_thresh)
contact_detected=true;
Expand Down Expand Up @@ -2053,20 +2052,20 @@ bool MotorThread::give(Bottle &options)
action[arm]->pushAction("open_hand");

wbdRecalibration();
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->enableContactDetection();
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->enableContactDetection();

bool contact_detected=false;
double t=Time::now();
while (!contact_detected && (Time::now()-t<5.0))
{
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->checkContact(contact_detected);
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->checkContact(contact_detected);
Time::delay(0.1);
}

if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->disableContactDetection();
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->disableContactDetection();

if(!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
setGazeIdle();
Expand Down Expand Up @@ -2392,8 +2391,8 @@ bool MotorThread::deploy(Bottle &options)
action[arm]->checkActionsDone(f,true);

wbdRecalibration();
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->enableContactDetection();
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->enableContactDetection();

ActionPrimitivesWayPoint wp;
deployOffs*=0.5;
Expand All @@ -2407,8 +2406,8 @@ bool MotorThread::deploy(Bottle &options)

action[arm]->checkActionsDone(f,true);
action[arm]->getPose(x,o);
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->disableContactDetection();
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->disableContactDetection();

if (!checkOptions(options,"no_head") && !checkOptions(options,"no_gaze"))
ctrl_gaze->lookAtFixationPoint(x);
Expand Down Expand Up @@ -2535,8 +2534,8 @@ bool MotorThread::calibTable(Bottle &options)
restoreContext(arm);

wbdRecalibration();
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->enableContactDetection();
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->enableContactDetection();

ActionPrimitivesWayPoint wp;
wp.x=deployPrepare; wp.o=reachAboveCata[arm];
Expand All @@ -2550,13 +2549,13 @@ bool MotorThread::calibTable(Bottle &options)

action[arm]->checkActionsDone(f,true);
action[arm]->pushWaitState(1.0);
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->disableContactDetection();
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->disableContactDetection();
action[arm]->enableReachingTimeout(reachingTimeout);

bool found;
if (typeid(action).name()==typeid(ActionPrimitivesLayer2).name())
dynamic_cast<ActionPrimitivesLayer2*>(action[arm])->checkContact(found);
if (auto* p = dynamic_cast<ActionPrimitivesLayer2*>(action[arm]))
p->checkContact(found);

if(found)
{
Expand Down

0 comments on commit 0bf92fe

Please sign in to comment.