diff --git a/source/RobotAPI/components/units/HeadIKUnit.cpp b/source/RobotAPI/components/units/HeadIKUnit.cpp index c040460811d3556b4d8a9267a5230e217f2b7c7f..86420ccb0189f575cb4442097243333cb47b3933 100644 --- a/source/RobotAPI/components/units/HeadIKUnit.cpp +++ b/source/RobotAPI/components/units/HeadIKUnit.cpp @@ -101,13 +101,13 @@ namespace armarx { drawer->setSphereDebugLayerVisu("HeadViewTarget", globalTarget, - DrawColor{0,1,1,0.7}, + DrawColor{1,0,0,0.7}, 15); } - + ARMARX_DEBUG << "new Head target set: " << *globalTarget; newTargetSet = true; } @@ -245,8 +245,10 @@ namespace armarx if(drawer && localRobot->hasRobotNode("Cameras")) { Vector3Ptr startPos = new Vector3(localRobot->getRobotNode("Cameras")->getGlobalPose()); - FramedVector3Ptr direction = new FramedVector3(Eigen::Vector3f(1, 0, 0),"Cameras", robotStateComponentPrx->getRobotName()); - drawer->setArrowDebugLayerVisu("HeadRealViewDirection", startPos, direction->toGlobal(localRobot), DrawColor{0,1,0,0.5}, 1500, 5); + drawer->setSphereDebugLayerVisu("HeadViewTarget", + startPos, + DrawColor{0,1,1,0.2}, + 17); } @@ -264,6 +266,7 @@ namespace armarx try { kinematicUnitPrx->switchControlMode(controlModes); + ARMARX_DEBUG << "setting new joint angles" << targetJointAngles; kinematicUnitPrx->setJointAngles(targetJointAngles); } catch (...) {