Skip to content
Snippets Groups Projects
Commit a5f65699 authored by Christoph Pohl's avatar Christoph Pohl
Browse files

Revert "Autoformatting" 

This reverts commit ffadd7db.
parent 9ddaf8a1
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
VirtualRobot/Manipulability/SingleChainManipulability.cpp
+ 135
− 205
View file @ a5f65699
......@@ -23,249 +23,179 @@
*/
#include "SingleChainManipulability.h"
#include <Visualization/VisualizationFactory.h>
#include <Visualization/VisualizationNode.h>
#include <Eigen/Dense>
#include <SimoxUtility/math/convert/pos_quat_to_mat4f.h>
#include "../IK/DifferentialIK.h"
#include "VirtualRobot/Robot.h"
#include "VirtualRobot/RobotNodeSet.h"
#include <Visualization/VisualizationFactory.h>
#include <Visualization/VisualizationNode.h>
#include "VirtualRobot/Robot.h"
#include <SimoxUtility/math/convert/pos_quat_to_mat4f.h>
namespace VirtualRobot
{
Eigen::MatrixXd
AbstractSingleChainManipulability::computeManipulability(const Eigen::MatrixXd& jacobian,
AbstractManipulability::Type type)
{
if (weightMatrix.rows() == 0)
{
int nbJoints = jacobian.cols();
weightMatrix.setIdentity(nbJoints, nbJoints);
}
Eigen::MatrixXd velocityManipulability =
jacobian * weightMatrix * weightMatrix.transpose() * jacobian.transpose();
switch (type)
{
case Velocity:
return velocityManipulability;
case Force:
return velocityManipulability.inverse();
default:
throw std::runtime_error("Unkown manipulability type");
}
Eigen::MatrixXd AbstractSingleChainManipulability::computeManipulability(const Eigen::MatrixXd &jacobian, AbstractManipulability::Type type) {
if (weightMatrix.rows() == 0){
int nbJoints = jacobian.cols();
weightMatrix.setIdentity(nbJoints, nbJoints);
}
Eigen::MatrixXd velocityManipulability = jacobian * weightMatrix * weightMatrix.transpose() * jacobian.transpose();
SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(
const RobotNodeSetPtr& rns,
Mode mode,
Type type,
Eigen::MatrixXd weightMatrixInit,
bool convertMMtoM) :
SingleRobotNodeSetManipulability(rns,
rns->getTCP(),
mode,
type,
RobotNodePtr(),
weightMatrixInit,
convertMMtoM)
{
switch (type) {
case Velocity:
return velocityManipulability;
case Force:
return velocityManipulability.inverse();
default:
throw std::runtime_error("Unkown manipulability type");
}
}
SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(
const RobotNodeSetPtr& rns,
const RobotNodePtr& node,
Mode mode,
Type type,
const RobotNodePtr& coordSystem,
Eigen::MatrixXd weightMatrixInit,
bool convertMMtoM) :
AbstractSingleChainManipulability(mode, type, weightMatrixInit),
rns(rns),
node(node),
coordSystem(coordSystem)
{
ik.reset(new DifferentialIK(rns, coordSystem, JacobiProvider::eSVDDampedDynamic));
ik->convertModelScalingtoM(convertMMtoM);
}
RobotNodeSetPtr
SingleRobotNodeSetManipulability::getRobotNodeSet()
{
return rns;
}
RobotPtr
SingleRobotNodeSetManipulability::getRobot()
{
return rns->getRobot();
}
void
SingleRobotNodeSetManipulability::setConvertMMtoM(bool value)
{
ik->convertModelScalingtoM(value);
}
Eigen::Vector3f
SingleRobotNodeSetManipulability::getLocalPosition()
{
return rns->getTCP()->getPositionInRootFrame();
}
SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(const RobotNodeSetPtr &rns, Mode mode, Type type, Eigen::MatrixXd weightMatrixInit, bool convertMMtoM)
: SingleRobotNodeSetManipulability(rns, rns->getTCP(), mode, type, RobotNodePtr(), weightMatrixInit, convertMMtoM)
{
}
Eigen::Vector3f
SingleRobotNodeSetManipulability::getGlobalPosition()
{
return rns->getTCP()->getGlobalPosition();
}
SingleRobotNodeSetManipulability::SingleRobotNodeSetManipulability(const RobotNodeSetPtr &rns, const RobotNodePtr &node, Mode mode, Type type, const RobotNodePtr &coordSystem, Eigen::MatrixXd weightMatrixInit, bool convertMMtoM)
: AbstractSingleChainManipulability(mode, type, weightMatrixInit), rns(rns), node(node), coordSystem(coordSystem)
{
ik.reset(new DifferentialIK(rns, coordSystem, JacobiProvider::eSVDDampedDynamic));
ik->convertModelScalingtoM(convertMMtoM);
}
Eigen::Matrix4f
SingleRobotNodeSetManipulability::getCoordinateSystem()
{
return coordSystem ? coordSystem->getGlobalPose() : Eigen::Matrix4f::Identity();
}
RobotNodeSetPtr SingleRobotNodeSetManipulability::getRobotNodeSet() {
return rns;
}
std::vector<std::string>
SingleRobotNodeSetManipulability::getJointNames()
{
return rns->getNodeNames();
}
RobotPtr SingleRobotNodeSetManipulability::getRobot() {
return rns->getRobot();
}
Eigen::VectorXd
SingleRobotNodeSetManipulability::getJointAngles()
{
return rns->getJointValuesEigen().cast<double>();
}
void SingleRobotNodeSetManipulability::setConvertMMtoM(bool value) {
ik->convertModelScalingtoM(value);
}
Eigen::Vector3f SingleRobotNodeSetManipulability::getLocalPosition() {
return rns->getTCP()->getPositionInRootFrame();
}
Eigen::VectorXd
SingleRobotNodeSetManipulability::getJointLimitsHigh()
Eigen::Vector3f SingleRobotNodeSetManipulability::getGlobalPosition() {
return rns->getTCP()->getGlobalPosition();
}
Eigen::Matrix4f SingleRobotNodeSetManipulability::getCoordinateSystem() {
return coordSystem ? coordSystem->getGlobalPose() : Eigen::Matrix4f::Identity();
}
std::vector<std::string> SingleRobotNodeSetManipulability::getJointNames() {
return rns->getNodeNames();
}
Eigen::VectorXd SingleRobotNodeSetManipulability::getJointAngles() {
return rns->getJointValuesEigen().cast<double>();
}
Eigen::VectorXd SingleRobotNodeSetManipulability::getJointLimitsHigh() {
Eigen::VectorXd jointLimitHi(rns->getSize());
for (size_t i = 0; i < rns->getSize(); i++)
{
Eigen::VectorXd jointLimitHi(rns->getSize());
for (size_t i = 0; i < rns->getSize(); i++)
RobotNodePtr rn = rns->getNode(i);
if (rn->isLimitless())
{
RobotNodePtr rn = rns->getNode(i);
if (rn->isLimitless())
{
jointLimitHi(i) = 0;
}
else
jointLimitHi(i) = rn->getJointLimitHi();
jointLimitHi(i) = 0;
}
return jointLimitHi;
}
Eigen::VectorXd
SingleRobotNodeSetManipulability::getJointLimitsLow()
{
Eigen::VectorXd jointLimitLo(rns->getSize());
for (size_t i = 0; i < rns->getSize(); i++)
else
jointLimitHi(i) = rn->getJointLimitHi();
}
return jointLimitHi;
}
Eigen::VectorXd SingleRobotNodeSetManipulability::getJointLimitsLow() {
Eigen::VectorXd jointLimitLo(rns->getSize());
for (size_t i = 0; i < rns->getSize(); i++)
{
RobotNodePtr rn = rns->getNode(i);
if (rn->isLimitless())
{
RobotNodePtr rn = rns->getNode(i);
if (rn->isLimitless())
{
jointLimitLo(i) = 0;
}
else
jointLimitLo(i) = rn->getJointLimitLo();
jointLimitLo(i) = 0;
}
return jointLimitLo;
else
jointLimitLo(i) = rn->getJointLimitLo();
}
return jointLimitLo;
}
Eigen::MatrixXd
SingleRobotNodeSetManipulability::computeJacobian(IKSolver::CartesianSelection mode)
{
return ik->getJacobianMatrix(node, mode).cast<double>();
}
Eigen::MatrixXd SingleRobotNodeSetManipulability::computeJacobian(IKSolver::CartesianSelection mode) {
return ik->getJacobianMatrix(node, mode).cast<double>();
}
SingleChainManipulability::SingleChainManipulability(
const Eigen::Matrix<double, 6, Eigen::Dynamic>& jacobian,
AbstractManipulability::Mode mode,
AbstractManipulability::Type type,
const Eigen::Matrix<double, Eigen::Dynamic, 1>& jointAngles,
const Eigen::VectorXd& jointLimitsHigh,
const Eigen::VectorXd& jointLimitsLow,
const std::vector<std::string>& jointNames,
const Eigen::Vector3f& globalPosition,
const Eigen::Vector3f& localPosition) :
AbstractSingleChainManipulability(mode, type),
jacobian(jacobian),
jointNames(jointNames),
globalPosition(globalPosition),
localPosition(localPosition),
jointAngles(jointAngles),
jointLimitsHigh(jointLimitsHigh),
jointLimitsLow(jointLimitsLow)
{
}
Eigen::Vector3f
SingleChainManipulability::getLocalPosition()
{
return localPosition;
}
Eigen::Vector3f
SingleChainManipulability::getGlobalPosition()
{
return globalPosition;
}
Eigen::Matrix4f
SingleChainManipulability::getCoordinateSystem()
{
return Eigen::Matrix4f::Identity();
}
SingleChainManipulability::SingleChainManipulability(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian,
AbstractManipulability::Mode mode, AbstractManipulability::Type type,
const Eigen::Matrix<double, Eigen::Dynamic, 1> &jointAngles, const Eigen::VectorXd &jointLimitsHigh, const Eigen::VectorXd &jointLimitsLow,
const std::vector<std::string> &jointNames,
const Eigen::Vector3f &globalPosition, const Eigen::Vector3f &localPosition) :
AbstractSingleChainManipulability(mode, type),
jacobian(jacobian),
jointNames(jointNames),
globalPosition(globalPosition),
localPosition(localPosition),
jointAngles(jointAngles),
jointLimitsHigh(jointLimitsHigh),
jointLimitsLow(jointLimitsLow)
{
}
std::vector<std::string>
SingleChainManipulability::getJointNames()
{
return jointNames;
}
Eigen::Vector3f SingleChainManipulability::getLocalPosition() {
return localPosition;
}
Eigen::VectorXd
SingleChainManipulability::getJointAngles()
{
return jointAngles;
}
Eigen::Vector3f SingleChainManipulability::getGlobalPosition() {
return globalPosition;
}
Eigen::VectorXd
SingleChainManipulability::getJointLimitsHigh()
{
return jointLimitsHigh;
}
Eigen::Matrix4f SingleChainManipulability::getCoordinateSystem() {
return Eigen::Matrix4f::Identity();
}
Eigen::VectorXd
SingleChainManipulability::getJointLimitsLow()
{
return jointLimitsLow;
}
std::vector<std::string> SingleChainManipulability::getJointNames() {
return jointNames;
}
void
SingleChainManipulability::setJacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic>& jacobian)
{
this->jacobian = jacobian;
}
Eigen::VectorXd SingleChainManipulability::getJointAngles() {
return jointAngles;
}
void
SingleChainManipulability::setLocalPosition(const Eigen::Vector3f& localPosition)
{
this->localPosition = localPosition;
}
Eigen::VectorXd SingleChainManipulability::getJointLimitsHigh() {
return jointLimitsHigh;
}
Eigen::VectorXd SingleChainManipulability::getJointLimitsLow() {
return jointLimitsLow;
}
void
SingleChainManipulability::setGlobalPosition(const Eigen::Vector3f& globalPosition)
{
this->globalPosition = globalPosition;
}
void SingleChainManipulability::setJacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic> &jacobian) {
this->jacobian = jacobian;
}
void SingleChainManipulability::setLocalPosition(const Eigen::Vector3f &localPosition) {
this->localPosition = localPosition;
}
void SingleChainManipulability::setGlobalPosition(const Eigen::Vector3f &globalPosition) {
this->globalPosition = globalPosition;
}
Eigen::MatrixXd SingleChainManipulability::computeJacobian(IKSolver::CartesianSelection mode) {
return GetJacobianSubMatrix(jacobian, mode);
}
Eigen::MatrixXd
SingleChainManipulability::computeJacobian(IKSolver::CartesianSelection mode)
{
return GetJacobianSubMatrix(jacobian, mode);
}
} // namespace VirtualRobot
}
VirtualRobot/Obstacle.cpp
+ 29
− 64
View file @ a5f65699
#include "Obstacle.h"
#include "CollisionDetection/CollisionChecker.h"
#include "CollisionDetection/CollisionModel.h"
#include "CollisionDetection/CollisionChecker.h"
#include "Nodes/RobotNode.h"
#include "Visualization/VisualizationFactory.h"
#include "Visualization/VisualizationNode.h"
#include <vector>
namespace VirtualRobot
{
......@@ -15,12 +15,8 @@ namespace VirtualRobot
// obstacle models start with 20000
int Obstacle::idCounter = 20000;
Obstacle::Obstacle(const std::string& name,
VisualizationNodePtr visualization,
CollisionModelPtr collisionModel,
const SceneObject::Physics& p,
CollisionCheckerPtr colChecker) :
GraspableSensorizedObject(name, visualization, collisionModel, p, colChecker)
Obstacle::Obstacle(const std::string& name, VisualizationNodePtr visualization, CollisionModelPtr collisionModel, const SceneObject::Physics& p, CollisionCheckerPtr colChecker)
: GraspableSensorizedObject(name, visualization, collisionModel, p, colChecker)
{
if (name == "")
{
......@@ -45,35 +41,25 @@ namespace VirtualRobot
}
}
Obstacle::Obstacle(const std::string& name,
const TriMeshModelPtr& trimesh,
const std::string& filename) :
Obstacle(TagTrimeshCtor{}, name, std::make_shared<VisualizationNode>(trimesh))
Obstacle::Obstacle(const std::string& name, const TriMeshModelPtr& trimesh, const std::string& filename)
: Obstacle(TagTrimeshCtor{}, name, std::make_shared<VisualizationNode>(trimesh))
{
getVisualization()->setFilename(filename, false);
getCollisionModel()->getVisualization()->setFilename(filename, false);
}
Obstacle::Obstacle(TagTrimeshCtor, const std::string& name, const VisualizationNodePtr& vis) :
Obstacle(name, vis, std::make_shared<CollisionModel>(vis))
{
}
Obstacle::Obstacle(TagTrimeshCtor, const std::string& name, const VisualizationNodePtr& vis)
: Obstacle(name, vis, std::make_shared<CollisionModel>(vis))
{}
Obstacle::~Obstacle() = default;
int
Obstacle::getID()
int Obstacle::getID()
{
return id;
}
VirtualRobot::ObstaclePtr
Obstacle::createBox(float width,
float height,
float depth,
VisualizationFactory::Color color,
std::string visualizationType,
CollisionCheckerPtr colChecker)
VirtualRobot::ObstaclePtr Obstacle::createBox(float width, float height, float depth, VisualizationFactory::Color color, std::string visualizationType, CollisionCheckerPtr colChecker)
{
ObstaclePtr result;
VisualizationFactoryPtr visualizationFactory;
......@@ -104,8 +90,7 @@ namespace VirtualRobot
if (!visu)
{
VR_ERROR << "Could not create box visualization with visu type " << visualizationType
<< std::endl;
VR_ERROR << "Could not create box visualization with visu type " << visualizationType << std::endl;
return result;
}
......@@ -126,11 +111,8 @@ namespace VirtualRobot
return result;
}
VirtualRobot::ObstaclePtr
Obstacle::createSphere(float radius,
VisualizationFactory::Color color,
std::string visualizationType,
CollisionCheckerPtr colChecker)
VirtualRobot::ObstaclePtr Obstacle::createSphere(float radius, VisualizationFactory::Color color, std::string visualizationType, CollisionCheckerPtr colChecker)
{
ObstaclePtr result;
VisualizationFactoryPtr visualizationFactory;
......@@ -159,8 +141,7 @@ namespace VirtualRobot
*/
if (!visu)
{
VR_ERROR << "Could not create sphere visualization with visu type " << visualizationType
<< std::endl;
VR_ERROR << "Could not create sphere visualization with visu type " << visualizationType << std::endl;
return result;
}
......@@ -182,12 +163,8 @@ namespace VirtualRobot
return result;
}
VirtualRobot::ObstaclePtr
Obstacle::createCylinder(float radius,
float height,
VisualizationFactory::Color color,
std::string visualizationType,
CollisionCheckerPtr colChecker)
VirtualRobot::ObstaclePtr Obstacle::createCylinder(float radius, float height, VisualizationFactory::Color color, std::string visualizationType, CollisionCheckerPtr colChecker)
{
ObstaclePtr result;
VisualizationFactoryPtr visualizationFactory;
......@@ -216,8 +193,7 @@ namespace VirtualRobot
*/
if (!visu)
{
VR_ERROR << "Could not create cylinder visualization with visu type "
<< visualizationType << std::endl;
VR_ERROR << "Could not create cylinder visualization with visu type " << visualizationType << std::endl;
return result;
}
......@@ -239,10 +215,8 @@ namespace VirtualRobot
return result;
}
VirtualRobot::ObstaclePtr
Obstacle::createFromMesh(TriMeshModelPtr mesh,
std::string visualizationType,
CollisionCheckerPtr colChecker)
VirtualRobot::ObstaclePtr Obstacle::createFromMesh(TriMeshModelPtr mesh, std::string visualizationType, CollisionCheckerPtr colChecker)
{
THROW_VR_EXCEPTION_IF(!mesh, "Null data");
......@@ -270,8 +244,7 @@ namespace VirtualRobot
if (!visu)
{
VR_ERROR << "Could not create sphere visualization with visu type " << visualizationType
<< std::endl;
VR_ERROR << "Could not create sphere visualization with visu type " << visualizationType << std::endl;
return result;
}
......@@ -291,8 +264,7 @@ namespace VirtualRobot
return result;
}
void
Obstacle::print(bool printDecoration /*= true*/)
void Obstacle::print(bool printDecoration /*= true*/)
{
if (printDecoration)
{
......@@ -310,8 +282,7 @@ namespace VirtualRobot
}
}
ObstaclePtr
Obstacle::clone(const std::string& name, CollisionCheckerPtr colChecker, float scaling) const
ObstaclePtr Obstacle::clone(const std::string& name, CollisionCheckerPtr colChecker, float scaling) const
{
VisualizationNodePtr clonedVisualizationNode;
......@@ -327,8 +298,7 @@ namespace VirtualRobot
clonedCollisionModel = collisionModel->clone(colChecker, scaling);
}
ObstaclePtr result(
new Obstacle(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker));
ObstaclePtr result(new Obstacle(name, clonedVisualizationNode, clonedCollisionModel, physics, colChecker));
result->setGlobalPose(getGlobalPose());
result->primitiveApproximation = primitiveApproximation;
......@@ -339,11 +309,7 @@ namespace VirtualRobot
return result;
}
std::string
Obstacle::toXML(const std::string& basePath,
int tabs,
const std::string& modelPathRelative,
bool storeSensors)
std::string Obstacle::toXML(const std::string& basePath, int tabs, const std::string& modelPathRelative, bool storeSensors)
{
std::stringstream ss;
std::string t = "\t";
......@@ -365,16 +331,15 @@ namespace VirtualRobot
return ss.str();
}
std::string
Obstacle::getFilename() const
std::string Obstacle::getFilename() const
{
return filename;
}
void
Obstacle::setFilename(const std::string& fname)
void Obstacle::setFilename(const std::string& fname)
{
this->filename = fname;
}
} // namespace VirtualRobot
} // namespace
VirtualRobot/Visualization/TriMeshModel.cpp
+ 117
− 184
View file @ a5f65699
This diff is collapsed.
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