diff --git a/source/RobotAPI/units/TCPControlUnit.cpp b/source/RobotAPI/units/TCPControlUnit.cpp
index 41a8a1f42e4e1f49942aad0d8f8823b54b59897e..f7d2c222e827a2e7b7ceff18a1079fa0c0fbefc5 100644
--- a/source/RobotAPI/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/units/TCPControlUnit.cpp
@@ -35,6 +35,7 @@ namespace armarx
{
TCPControlUnit::TCPControlUnit() :
+ maxJointVelocity(30.f/180*3.141),
cycleTime(30)
{
@@ -57,15 +58,18 @@ namespace armarx
remoteRobot.reset( new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
- // dIK = new VirtualRobot::DifferentialIK(robotNodeSet);
+ // dIK = new VirtualRobot::DifferentialIK(robotNodeSet);
FramedVector3Ptr tcpVel = new FramedVector3();
- tcpVel->z = -10;
+ tcpVel->x = -30;
tcpVel->frame = "Root";
FramedVector3Ptr tcpOriVel = new FramedVector3();
tcpOriVel->frame = "Root";
- setTCPVelocity("LeftArm", tcpVel, tcpOriVel);
+ setTCPVelocity("LeftArm", tcpVel, NULL);
+// setTCPVelocity("RightArm", tcpVel, NULL);
+// setTCPVelocity("RightLeg", tcpVel, NULL);
+// setTCPVelocity("LeftLeg", tcpVel, NULL);
request();
}
@@ -121,26 +125,32 @@ namespace armarx
void TCPControlUnit::release(const Ice::Current & c)
{
ARMARX_IMPORTANT << "Releasing TCPControlUnit";
+ kinematicUnitPrx->stop();
execTask->stop();
}
void TCPControlUnit::periodicExec()
{
-// remoteRobot.reset( new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
+ // remoteRobot.reset( new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
FramedVector3Map::const_iterator itTrans = targetTranslationVelocities.begin();
for(; itTrans != targetTranslationVelocities.end(); itTrans++)
{
-
+ Eigen::VectorXf jointDelta;
FramedVector3Ptr targetTranslationVelocity = FramedVector3Ptr::dynamicCast(itTrans->second);
+ if(targetTranslationVelocity->getFrame() == "")
+ continue;
+ VirtualRobot::RobotNodeSetPtr robotNodeSet = remoteRobot->getRobotNodeSet(itTrans->first);
+ std::string refFrame = targetTranslationVelocity->getFrame();
+
+
FramedVector3Map::const_iterator itOri = targetOrientationVelocitiesRPY.find(itTrans->first);
- if(itOri != targetOrientationVelocitiesRPY.end())
+ if(itOri != targetOrientationVelocitiesRPY.end() && itOri->second)
{
FramedVector3Ptr targetOrientationVelocitiesRPY = FramedVector3Ptr::dynamicCast(itOri->second);;
- if(targetTranslationVelocity->getFrame() == ""
- || targetOrientationVelocitiesRPY->getFrame() == "")
+ if(targetOrientationVelocitiesRPY->getFrame() == "")
continue;
@@ -148,9 +158,9 @@ namespace armarx
&& targetOrientationVelocitiesRPY->toEigen().norm() == 0)
continue;
- VirtualRobot::RobotNodeSetPtr robotNodeSet = remoteRobot->getRobotNodeSet(itTrans->first);
- std::string refFrame = targetTranslationVelocity->getFrame();
+
+
FramedVector3Ptr lVecTrans = FramedVector3::ChangeFrame(remoteRobot, *targetTranslationVelocity, refFrame);
FramedVector3Ptr lVecOri = FramedVector3::ChangeFrame(remoteRobot, *targetOrientationVelocitiesRPY, refFrame);
@@ -158,47 +168,82 @@ namespace armarx
Eigen::VectorXf tcpDelta(6);
tcpDelta.head(3) = lVecTrans->toEigen();
tcpDelta.tail(3) = lVecOri->toEigen();
- Eigen::VectorXf jointDelta = calcJointVelocities(robotNodeSet, tcpDelta, refFrame);
-
-
- // build name value map
- NameValueMap targetVelocities;
- NameControlModeMap controlModes;
- const std::vector< VirtualRobot::RobotNodePtr > nodes = robotNodeSet->getAllRobotNodes();
- std::vector< VirtualRobot::RobotNodePtr >::const_iterator iter = nodes.begin();
- int i = 0;
- ARMARX_VERBOSE_S << "Setting tcp velocity to:" << jointDelta;
- while (iter != nodes.end() && i < jointDelta.rows())
- {
- targetVelocities.insert(std::make_pair((*iter)->getName(), jointDelta(i)));
-
- controlModes.insert(std::make_pair((*iter)->getName(), eVelocityControl));
- i++;
- iter++;
- };
-
- // execute velocities
- kinematicUnitPrx->switchControlMode(controlModes);
- kinematicUnitPrx->setJointVelocities(targetVelocities);
+ jointDelta = calcJointVelocities(robotNodeSet, tcpDelta, refFrame, VirtualRobot::IKSolver::All);
+
+ // for (int r = 0; r < jointDelta.rows(); ++r) {
+ // if(jointDelta(r) > maxJointVelocity)
+ // {
+ //
+ // break;
+ // }
+ // }
+
+
}
+ else // only position control
+ {
+ if(targetTranslationVelocity->toEigen().norm() == 0)
+ continue;
+
+ FramedVector3Ptr lVecTrans = FramedVector3::ChangeFrame(remoteRobot, *targetTranslationVelocity, refFrame);
+
+ Eigen::VectorXf tcpDelta(3);
+ tcpDelta = lVecTrans->toEigen();
+ jointDelta = calcJointVelocities(robotNodeSet, tcpDelta, refFrame, VirtualRobot::IKSolver::Position);
+
+
+ }
+
+ Eigen::Vector3f currentTCPPosition = robotNodeSet->getTCP()->getGlobalPose().block(0,3,3,1);
+ ARMARX_INFO << "Current TCP Position: \n" << currentTCPPosition;
+ ARMARX_INFO << "ActualTCPVelocity: " << (currentTCPPosition - lastTCPPose.block(0,3,3,1))/(0.001*cycleTime);
+ lastTCPPose = robotNodeSet->getTCP()->getGlobalPose();
+
+ // build name value map
+ NameValueMap targetVelocities;
+ NameControlModeMap controlModes;
+ const std::vector< VirtualRobot::RobotNodePtr > nodes = robotNodeSet->getAllRobotNodes();
+ std::vector< VirtualRobot::RobotNodePtr >::const_iterator iter = nodes.begin();
+ int i = 0;
+ while (iter != nodes.end() && i < jointDelta.rows())
+ {
+ targetVelocities.insert(std::make_pair((*iter)->getName(), jointDelta(i)));
+
+ controlModes.insert(std::make_pair((*iter)->getName(), eVelocityControl));
+ i++;
+ iter++;
+ };
+
+ // execute velocities
+ kinematicUnitPrx->switchControlMode(controlModes);
+ kinematicUnitPrx->setJointVelocities(targetVelocities);
}
+
+
}
- Eigen::VectorXf TCPControlUnit::calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, std::string refFrame)
+ Eigen::VectorXf TCPControlUnit::calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, std::string refFrame, VirtualRobot::IKSolver::CartesianSelection mode)
{
+ IceUtil::Time startCreation = IceUtil::Time::now();
+
// TODO: Turn into member variable!
- VirtualRobot::DifferentialIKPtr dIK(new VirtualRobot::DifferentialIK(robotNodeSet, remoteRobot->getRobotNode(refFrame)));
+ VirtualRobot::DifferentialIKPtr dIK(new VirtualRobot::DifferentialIK(robotNodeSet, remoteRobot->getRobotNode(refFrame)/*, VirtualRobot::JacobiProvider::eTranspose*/));
VirtualRobot::RobotNodePtr tcpNode = robotNodeSet->getTCP();
+ ARMARX_INFO << "creation: " << (IceUtil::Time::now() - startCreation).toMilliSeconds();
- Eigen::MatrixXf Ji = dIK->getPseudoInverseJacobianMatrix(tcpNode, VirtualRobot::IKSolver::All);
+ IceUtil::Time startJacobian = IceUtil::Time::now();
+ Eigen::MatrixXf J = dIK->getJacobianMatrix(tcpNode, mode);
+ ARMARX_INFO << "Jacobian: " << (IceUtil::Time::now() - startJacobian).toMilliSeconds();
+ Eigen::MatrixXf Ji = dIK->getPseudoInverseJacobianMatrix(tcpNode, mode);
- ARMARX_CHECK_EXPRESSION_W_HINT(tcpDelta.cols() != 6, "TCPDelta Vector must have 6 items")
+ ARMARX_INFO << "TCP Delta (" << tcpDelta.rows() << ")\n" << tcpDelta;
+ ARMARX_CHECK_EXPRESSION_W_HINT(tcpDelta.rows() == 6 || tcpDelta.rows() == 3, "TCPDelta Vector must have 6 or 3 items");
- // calculat joint error
- Eigen::VectorXf deltaJoint = Ji * tcpDelta;
+ // calculat joint error
+ Eigen::VectorXf deltaJoint = Ji * tcpDelta;
return deltaJoint;
}
diff --git a/source/RobotAPI/units/TCPControlUnit.h b/source/RobotAPI/units/TCPControlUnit.h
index 02a768d5f629ec5c1c886d828778f8a2653acbd3..5ecb7686a9abcce752487c960edab0e59faeb086 100644
--- a/source/RobotAPI/units/TCPControlUnit.h
+++ b/source/RobotAPI/units/TCPControlUnit.h
@@ -44,6 +44,7 @@ namespace armarx {
{
defineRequiredProperty<std::string>("KinematicUnitName","Name of the KinematicUnit");
defineRequiredProperty<std::string>("RobotNodeSetName","Name of the KinematicUnit");
+ defineOptionalProperty<std::string>("MaxJointVelocity","Maximal joint velocity in rad/sec");
}
@@ -83,13 +84,16 @@ namespace armarx {
private:
void periodicExec();
- Eigen::VectorXf calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, std::string refFrame);
+ Eigen::VectorXf calcJointVelocities(VirtualRobot::RobotNodeSetPtr robotNodeSet, Eigen::VectorXf tcpDelta, std::string refFrame, VirtualRobot::IKSolver::CartesianSelection mode);
+
+ float maxJointVelocity;
+ int cycleTime;
+ Eigen::Matrix4f lastTCPPose;
FramedVector3Map targetTranslationVelocities;
FramedVector3Map targetOrientationVelocitiesRPY;
PeriodicTask<TCPControlUnit>::pointer_type execTask;
- int cycleTime;
RobotStateComponentInterfacePrx robotStateComponentPrx;
KinematicUnitInterfacePrx kinematicUnitPrx;