void LinkPointActor::setSize(osg::Vec3 size)
{
_size = size;
if (_transformNode.valid())
{
_transformNode->setScale(_size);
setObjectTransform(_basePosition);
}
}
/// @todo verify that the ordering of the jacobian returned is accurate against VrepVFController.hpp
/// @return jacobian in ColumnMajor format where each row is a joint from base to tip, first 3 columns are translation component, last 3 columns are rotation component
Eigen::MatrixXf getJacobian(vrep::VrepRobotArmDriver& driver)
{
// Initialize Variables for update
auto jointHandles_ = driver.getJointHandles();
int numJoints =jointHandles_.size();
std::vector<float> ikCalculatedJointValues(numJoints,0);
vrep::VrepRobotArmDriver::State currentArmState_;
driver.getState(currentArmState_);
/// @todo get target position, probably relative to base
const auto& handleParams = driver.getVrepHandleParams();
auto ikGroupHandle_ = std::get<vrep::VrepRobotArmDriver::RobotIkGroup>(handleParams);
auto target = std::get<vrep::VrepRobotArmDriver::RobotTargetName>(handleParams);
auto tip = std::get<vrep::VrepRobotArmDriver::RobotTipName>(handleParams);
// save the current tip to target transform
Eigen::Affine3d tipToTarget =getObjectTransform( target,tip);
// set the current transform to the identity so simCheckIkGroup won't fail
// @see http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=3967
// for details about this issue.
setObjectTransform( target,tip,Eigen::Affine3d::Identity());
// debug:
// std::cout << "TipToTargetTransform:\n" << tipToTarget.matrix() << "\n";
/// Run inverse kinematics, but all we really want is the jacobian
/// @todo find version that only returns jacobian
/// @see http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simCheckIkGroup
auto ikcalcResult =
simCheckIkGroup(ikGroupHandle_
,numJoints
,&jointHandles_[0]
,&ikCalculatedJointValues[0]
,NULL /// @todo do we need to use these options?
);
/// @see http://www.coppeliarobotics.com/helpFiles/en/apiConstants.htm#ikCalculationResults
if(ikcalcResult!=sim_ikresult_success && ikcalcResult != sim_ikresult_not_performed)
{
BOOST_LOG_TRIVIAL(error) << "VrepInverseKinematicsController: didn't run inverse kinematics";
return Eigen::MatrixXf();
}
setObjectTransform( target,tip,tipToTarget);
// debug verifying that get and set object transform don't corrupt underlying data
// Eigen::Affine3d tipToTarget2 =getObjectTransform( target,tip);
// std::cout << "\ntiptotarget\n" << tipToTarget.matrix();
// std::cout << "\ntiptotarget2\n" << tipToTarget2.matrix();
// Get the Jacobian
int jacobianSize[2];
float* jacobian=simGetIkGroupMatrix(ikGroupHandle_,0,jacobianSize);
/// @todo FIX HACK jacobianSize include orientation component, should be 7x6 instead of 7x3
#ifdef IGNORE_ROTATION
jacobianSize[1] = 3;
#endif
// Transfer the Jacobian to cisst
// jacobianSize[1] represents the row count of the Jacobian
// (i.e. the number of equations or the number of joints involved
// in the IK resolution of the given kinematic chain)
// Joints appear from tip to base.
// jacobianSize[0] represents the column count of the Jacobian
// (i.e. the number of constraints, e.g. x,y,z,alpha,beta,gamma,jointDependency)
// The Jacobian data is in RowMajor order, i.e.:
// https://en.wikipedia.org/wiki/Row-major_order
std::string str;
// jacobianSize[1] == eigen ColMajor "rows" , jacobianSize[0] == eigen ColMajor "cols"
Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> > mtestjacobian(jacobian,jacobianSize[1],jacobianSize[0]);
Eigen::MatrixXf eigentestJacobian = mtestjacobian.rowwise().reverse().transpose();
return eigentestJacobian;
}
