std::vector<Eigen::Matrix4f > Trajectory::createWorkspaceTrajectory( VirtualRobot::RobotNodePtr r )
{
VR_ASSERT(rns);
if (!r)
r = rns->getTCP();
VR_ASSERT(r);
RobotPtr robot = rns->getRobot();
VR_ASSERT(robot);
Eigen::VectorXf jv;
rns->getJointValues(jv);
std::vector<Eigen::Matrix4f > result;
for (size_t i = 0; i < path.size(); i++)
{
// get tcp coords:
robot->setJointValues(rns,path[i]);
Eigen::Matrix4f m;
result.push_back(r->getGlobalPose());
}
robot->setJointValues(rns,jv);
return result;
}
VirtualRobot::CollisionModelPtr CollisionModel::CreateUnitedCollisionModel(const std::vector<CollisionModelPtr>& colModels)
{
VR_ASSERT(colModels.size() > 0);
CollisionCheckerPtr colChecker = colModels[0]->getCollisionChecker();
std::vector<VisualizationNodePtr> visus;
for (size_t i = 0; i < colModels.size(); i++)
{
VisualizationNodePtr v = colModels[i]->getVisualization();
if (v)
{
visus.push_back(v);
}
VR_ASSERT(colModels[i]->getCollisionChecker() == colChecker);
}
if (visus.size() == 0)
{
return CollisionModelPtr();
}
VisualizationNodePtr vc = VisualizationNode::CreateUnitedVisualization(visus);
return CollisionModelPtr(new CollisionModel(vc, "", colChecker));
}
void DynamicsRobot::createDynamicsNode( VirtualRobot::RobotNodePtr node )
{
VR_ASSERT(node);
// check if already created
if (hasDynamicsRobotNode(node))
return;
DynamicsWorldPtr dw = DynamicsWorld::GetWorld();
DynamicsObjectPtr drn = dw->CreateDynamicsObject(node);
VR_ASSERT(drn);
dynamicRobotNodes[node] = drn;
}
void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue , float jointVelocity)
{
VR_ASSERT(robot);
VR_ASSERT(node);
VR_ASSERT(robot->hasRobotNode(node));
robotNodeActuationTarget target;
target.actuation = ePositionVelocity;
target.node = node;
target.jointValueTarget = jointValue;
target.jointVelocityTarget = jointVelocity;
target.jointTorqueTarget = 0.0f;
actuationTargets[node] = target;
}
DynamicsObjectPtr DynamicsRobot::getDynamicsRobotNode( VirtualRobot::RobotNodePtr node )
{
VR_ASSERT(node);
if (dynamicRobotNodes.find(node) == dynamicRobotNodes.end())
return DynamicsObjectPtr();
return dynamicRobotNodes[node];
}
bool DynamicsRobot::isNodeActuated( VirtualRobot::RobotNodePtr node )
{
VR_ASSERT(node);
if (actuationTargets.find(node) == actuationTargets.end())
return false;
return actuationTargets[node].actuation!=eDisabled;
}
float DynamicsRobot::getNodeTarget( VirtualRobot::RobotNodePtr node )
{
VR_ASSERT(node);
if (actuationTargets.find(node) == actuationTargets.end())
return 0.0f;
return actuationTargets[node].jointValueTarget;
}
void RobotNode::setJointValue(float q)
{
RobotPtr r = getRobot();
VR_ASSERT(r);
WriteLockPtr lock = r->getWriteLock();
setJointValueNoUpdate(q);
updatePose();
}
void RobotNodeSet::setJointValues( const RobotConfigPtr jointValues )
{
VR_ASSERT(jointValues);
RobotPtr rob = robot.lock();
VR_ASSERT(rob);
WriteLockPtr lock = rob->getWriteLock();
for (unsigned int i=0;i<robotNodes.size();i++)
{
if (jointValues->hasConfig(robotNodes[i]->getName()))
robotNodes[i]->setJointValueNoUpdate(jointValues->getConfig(robotNodes[i]->getName()));
}
if (kinematicRoot)
kinematicRoot->updatePose();
else
rob->applyJointValues();
}
void DynamicsRobot::actuateNodeTorque( VirtualRobot::RobotNodePtr node, float jointTorque )
{
VR_ASSERT(robot);
VR_ASSERT(node);
VR_ASSERT(robot->hasRobotNode(node));
//if (!hasDynamicsRobotNode(node))
// createDynamicsNode(node);
//DynamicsObjectPtr dnyRN = getDynamicsRobotNode(node);
robotNodeActuationTarget target;
target.actuation = eTorque;
target.node = node;
target.jointValueTarget = 0.0f;
target.jointVelocityTarget = 0.0f;
target.jointTorqueTarget = jointTorque;
//target.dynNode = dnyRN;
actuationTargets[node] = target;
}
CoMIK::CoMIK(RobotNodeSetPtr rnsJoints, RobotNodeSetPtr rnsBodies, RobotNodePtr coordSystem, int dimensions)
: JacobiProvider(rnsJoints), coordSystem(coordSystem)
{
VR_ASSERT(rns);
VR_ASSERT(rnsBodies);
checkImprovement = false;
this->rnsBodies = rnsBodies;
numDimensions = dimensions;
bodyNodes = rnsBodies->getAllRobotNodes();
for (size_t i = 0; i < bodyNodes.size(); i++)
{
// get all joints that influence the body
std::vector<RobotNodePtr> parentsN = bodyNodes[i]->getAllParents(rns);
bodyNodeParents[bodyNodes[i]] = parentsN;
}
if (rnsBodies->getMass()==0)
{
VR_ERROR << "The RNS does not contain any bodies or masses are not specified (mass==0)" << endl;
}
}
RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot,
const std::string &name,
const std::vector< std::string > &robotNodeNames,
const std::string &kinematicRootName,
const std::string &tcpName,
bool registerToRobot)
{
VR_ASSERT (robot!=NULL);
std::vector< RobotNodePtr > robotNodes;
if (robotNodeNames.empty())
{
VR_WARNING << " No robot nodes in set..." << endl;
}
else
{
for (unsigned int i=0;i<robotNodeNames.size();i++)
{
RobotNodePtr node = robot->getRobotNode(robotNodeNames[i]);
THROW_VR_EXCEPTION_IF(!node, "No robot node with name " << robotNodeNames[i] << " found...");
robotNodes.push_back(node);
}
}
RobotNodePtr kinematicRoot;
if (!kinematicRootName.empty())
{
RobotNodePtr node = robot->getRobotNode(kinematicRootName);
THROW_VR_EXCEPTION_IF(!node, "No root node with name " << kinematicRootName << " found...");
kinematicRoot = node;
}
else
{
if (!robotNodes.empty())
kinematicRoot = robotNodes[0];
}
RobotNodePtr tcp;
if (!tcpName.empty())
{
RobotNodePtr node = robot->getRobotNode(tcpName);
THROW_VR_EXCEPTION_IF(!node, "No tcp node with name " << tcpName << " found...");
tcp = node;
}
else
{
if (!robotNodes.empty())
tcp = robotNodes[robotNodes.size()-1];
}
RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(robot, name, robotNodes, kinematicRoot, tcp, registerToRobot);
return rns;
}
RobotNodeSetPtr RobotNodeSet::createRobotNodeSet(RobotPtr robot,
const std::string &name,
const std::vector< RobotNodePtr > &robotNodes,
const RobotNodePtr kinematicRoot,
const RobotNodePtr tcp,
bool registerToRobot)
{
VR_ASSERT (robot!=NULL);
if (robotNodes.empty() || !robotNodes[0])
{
VR_WARNING << " No robot nodes in set..." << endl;
} else
{
CollisionCheckerPtr cc = robotNodes[0]->getCollisionChecker();
for (unsigned int i=1;i<robotNodes.size();i++)
{
if (robotNodes[0]->getRobot() != robotNodes[i]->getRobot())
{
THROW_VR_EXCEPTION("Robot nodes belong to different robots");
}
if (cc != robotNodes[i]->getCollisionChecker())
{
THROW_VR_EXCEPTION("Robot nodes belong to different collision checkers");
}
}
}
RobotNodePtr kinematicRootNode = kinematicRoot;
if (!kinematicRootNode)
{
kinematicRootNode = robot->getRootNode();
}
RobotNodePtr tcpNode = tcp;
if (!tcpNode)
{
THROW_VR_EXCEPTION_IF(robotNodes.empty(), "can not determine the tcp node need for creating a RobotNodeSet");
tcpNode = robotNodes[robotNodes.size()-1];
}
RobotNodeSetPtr rns(new RobotNodeSet(name, robot, robotNodes, kinematicRootNode, tcpNode));
if (registerToRobot)
{
THROW_VR_EXCEPTION_IF(robot->hasRobotNodeSet(rns), "RobotNodeSet with name " << rns->getName() << " already present in the robot");
robot->registerRobotNodeSet(rns);
}
return rns;
}
void DynamicsRobot::actuateNode( VirtualRobot::RobotNodePtr node, float jointValue )
{
VR_ASSERT(robot);
VR_ASSERT(node);
VR_ASSERT(robot->hasRobotNode(node));
// if node is a joint without model, there is no dyn node!
//DynamicsObjectPtr dnyRN;
//if (hasDynamicsRobotNode(node))
// dnyRN = getDynamicsRobotNode(node);
// createDynamicsNode(node);
robotNodeActuationTarget target;
target.actuation = ePosition;
target.node = node;
target.jointValueTarget = jointValue;
target.jointVelocityTarget = 0.0f;
target.jointTorqueTarget = 0.0f;
//target.dynNode = dnyRN;
actuationTargets[node] = target;
}
void RobotNodeSet::setJointValues(const Eigen::VectorXf &jointValues)
{
THROW_VR_EXCEPTION_IF(jointValues.rows() != robotNodes.size(), "Wrong vector dimension (robotNodes:" << robotNodes.size() << ", jointValues: " << jointValues.size() << ")" << endl);
RobotPtr rob = robot.lock();
VR_ASSERT(rob);
WriteLockPtr lock = rob->getWriteLock();
for (unsigned int i=0;i<robotNodes.size();i++)
{
robotNodes[i]->setJointValueNoUpdate(jointValues[i]);
}
if (kinematicRoot)
kinematicRoot->updatePose();
else
rob->applyJointValues();
}
SupportPolygon::SupportPolygon(SceneObjectSetPtr contactModels)
: contactModels(contactModels)
{
floor = MathTools::getFloorPlane();
VR_ASSERT(this->contactModels);
for (size_t i = 0; i < contactModels->getSize(); i++)
{
if (contactModels->getSceneObject((int)i)->getCollisionModel())
{
colModels.push_back(contactModels->getSceneObject((int)i)->getCollisionModel());
}
}
THROW_VR_EXCEPTION_IF(colModels.size() == 0, "RobotNodeSet does not contain any collision models");
}
VirtualRobot::TriMeshModelPtr SphereApproximator::generateTriMesh(const SphereApproximation& a)
{
Eigen::Vector3f v1, v2, v3;
MathTools::TriangleFace f;
int nVertices = (int)a.vertices.size();
int nFaces = (int)a.faces.size();
VR_ASSERT(nVertices > 0);
int nVertexCount = 0;
TriMeshModelPtr tr(new TriMeshModel());
for (int i = 0; i < nFaces; i++)
{
f = a.faces.at(i);
v1 = a.vertices.at(f.id1);
v2 = a.vertices.at(f.id2);
v3 = a.vertices.at(f.id3);
tr->addTriangleWithFace(v1, v3, v2);
}
return tr;
}
RobotConfigPtr RobotConfig::clone( RobotPtr newRobot )
{
if (!newRobot)
newRobot = robot.lock();
VR_ASSERT (newRobot);
std::map< RobotNodePtr, float > newConfigs;
std::map< RobotNodePtr, float >::iterator i = configs.begin();
while (i!=configs.end())
{
RobotNodePtr rn = newRobot->getRobotNode(i->first->getName());
if (!rn)
{
VR_WARNING << "Could not completely clone RobotConfig " << name << " because new robot does not know a RobotNode with name " << i->first->getName() << endl;
} else
{
newConfigs[rn] = i->second;
}
i++;
}
RobotConfigPtr result(new RobotConfig(newRobot,name,newConfigs));
return result;
}
void PathProcessingThread::workingMethod()
{
if (!threadStarted)
{
VR_WARNING << "Thread should be in started mode?!" << endl;
}
VR_ASSERT(pathProcessor);
CSpacePathPtr res = pathProcessor->optimize(optimizeSteps);
mutex.lock();
if (res)
processingFinished = true;
else
processingFinished = false;
resultPath = res;
// the thread ends here
threadStarted = false;
mutex.unlock();
}
void MeshConverter::checkAndSplitVertex(VirtualRobot::TriMeshModelPtr tm, int faceIdx, float maxDist)
{
VR_ASSERT(tm);
VR_ASSERT(faceIdx >= 0 && faceIdx < (int)tm->faces.size());
float d12, d13, d23;
Eigen::Vector3f v1, v2, v3;
v1 = tm->vertices[tm->faces[faceIdx].id1];
v2 = tm->vertices[tm->faces[faceIdx].id2];
v3 = tm->vertices[tm->faces[faceIdx].id3];
Eigen::Vector3f v4;
unsigned int id4;
size_t checkFaceIdx;
d12 = (v1 - v2).norm();
d23 = (v2 - v3).norm();
d13 = (v1 - v3).norm();
if (d12 > maxDist || d23 > maxDist || d13 > maxDist)
{
// split at longest edge
if (d12 >= d23 && d12 >= d13)
{
v4 = v1 + (v2 - v1) * 0.5f;
tm->addVertex(v4);
id4 = tm->vertices.size() - 1;
// add new face
MathTools::TriangleFace face;
face.id1 = id4;
face.id2 = tm->faces[faceIdx].id2;
face.id3 = tm->faces[faceIdx].id3;
face.normal = tm->faces[faceIdx].normal;
tm->addFace(face);
// update current face
tm->faces[faceIdx].id2 = id4;
}
else if (d23 >= d12 && d23 >= d13)
{
v4 = v2 + (v3 - v2) * 0.5f;
tm->addVertex(v4);
id4 = tm->vertices.size() - 1;
// add new face
MathTools::TriangleFace face;
face.id1 = tm->faces[faceIdx].id1;
face.id2 = id4;
face.id3 = tm->faces[faceIdx].id3;
face.normal = tm->faces[faceIdx].normal;
tm->addFace(face);
// update current face
tm->faces[faceIdx].id3 = id4;
}
else
{
v4 = v3 + (v1 - v3) * 0.5f;
tm->addVertex(v4);
id4 = tm->vertices.size() - 1;
// add new face
MathTools::TriangleFace face;
face.id1 = tm->faces[faceIdx].id1;
face.id2 = tm->faces[faceIdx].id2;
face.id3 = id4;
face.normal = tm->faces[faceIdx].normal;
tm->addFace(face);
// update current face
tm->faces[faceIdx].id1 = id4;
}
checkFaceIdx = tm->faces.size() - 1;
// check new face
checkAndSplitVertex(tm, checkFaceIdx, maxDist);
// recursively check current face
checkAndSplitVertex(tm, faceIdx, maxDist);
}
}
void DynamicsRobot::actuateNode(const std::string &node, float jointValue )
{
VR_ASSERT(robot);
VR_ASSERT(robot->hasRobotNode(node));
actuateNode(robot->getRobotNode(node),jointValue);
}
void Trajectory::addPoint(const Eigen::VectorXf &c)
{
VR_ASSERT (c.rows() == dimension);
path.push_back(c);
}
float PoseQualityMeasurement::getPoseQuality( const Eigen::VectorXf &direction )
{
VR_ASSERT(direction.rows()==3 || direction.rows()==6);
VR_WARNING << "Please use derived classes..." << endl;
return 0.0f;
}
