Skip to content
Snippets Groups Projects
Commit 6e48734b authored by Mirko Wächter's avatar Mirko Wächter
Browse files

Extended TCPControlUnit

parent 549acb5a
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/units/TCPControlUnit.cpp
+ 83
− 38
View file @ 6e48734b
......@@ -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;
}
......
source/RobotAPI/units/TCPControlUnit.h
+ 6
− 2
View file @ 6e48734b
......@@ -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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment