Exporter::Result Exporter::exportLight(NiNodeRef parent, INode *node, GenLight* light)
{
TimeValue t = 0;
NiLightRef niLight;
switch (light->Type())
{
case OMNI_LIGHT:
{
if (light->GetAmbientOnly())
{
niLight = new NiAmbientLight();
}
else
{
NiPointLightRef pointLight = new NiPointLight();
float atten = light->GetAtten(t, ATTEN_START);
switch (light->GetDecayType())
{
case 0: pointLight->SetConstantAttenuation(1.0f); break;
case 1: pointLight->SetLinearAttenuation( atten / 4.0f ); break;
case 2: pointLight->SetQuadraticAttenuation( sqrt(atten / 4.0f) ); break;
}
niLight = StaticCast<NiLight>(pointLight);
}
}
break;
case TSPOT_LIGHT:
case FSPOT_LIGHT:
niLight = new NiSpotLight();
break;
case DIR_LIGHT:
case TDIR_LIGHT:
niLight = new NiDirectionalLight();
break;
}
if (niLight == NULL)
return Skip;
niLight->SetName(node->GetName());
Matrix3 tm = getObjectTransform(node, t, !mFlattenHierarchy);
niLight->SetLocalTransform( TOMATRIX4(tm, false) );
niLight->SetDimmer( light->GetIntensity(0) );
Color3 rgbcolor = TOCOLOR3( light->GetRGBColor(0) );
if (light->GetAmbientOnly())
{
niLight->SetDiffuseColor(Color3(0,0,0));
niLight->SetSpecularColor(Color3(0,0,0));
niLight->SetAmbientColor(rgbcolor);
}
else
{
niLight->SetDiffuseColor(rgbcolor);
niLight->SetSpecularColor(rgbcolor);
niLight->SetAmbientColor(Color3(0,0,0));
}
parent->AddChild( DynamicCast<NiAVObject>(niLight) );
return Ok;
}
Exporter::Result Exporter::exportMesh(NiNodeRef &ninode, INode *node, TimeValue t)
{
ObjectState os = node->EvalWorldState(t);
bool local = !mFlattenHierarchy;
TriObject *tri = (TriObject *)os.obj->ConvertToType(t, Class_ID(TRIOBJ_CLASS_ID, 0));
if (!tri)
return Skip;
Mesh *copymesh = NULL;
Mesh *mesh = &tri->GetMesh();
Matrix3 mtx(true), rtx(true);
if (Exporter::mCollapseTransforms)
{
mtx = GetNodeLocalTM(node, t);
mtx.NoTrans();
Quat q(mtx);
q.MakeMatrix(rtx);
mesh = copymesh = new Mesh(*mesh);
{
int n = mesh->getNumVerts();
for ( unsigned int i = 0; i < n; ++i ) {
Point3& vert = mesh->getVert(i);
vert = mtx * vert;
}
mesh->checkNormals(TRUE);
#if VERSION_3DSMAX > ((5000<<16)+(15<<8)+0) // Version 6+
MeshNormalSpec *specNorms = mesh->GetSpecifiedNormals ();
if (NULL != specNorms) {
specNorms->CheckNormals();
for ( unsigned int i = 0; i < specNorms->GetNumNormals(); ++i ) {
Point3& norm = specNorms->Normal(i);
norm = (rtx * norm).Normalize();
}
}
#endif
}
}
// Note that calling setVCDisplayData will clear things like normals so we set this up first
vector<Color4> vertColors;
if (mVertexColors)
{
bool hasvc = false;
if (mesh->mapSupport(MAP_ALPHA))
{
mesh->setVCDisplayData(MAP_ALPHA); int n = mesh->getNumVertCol();
if (n > vertColors.size())
vertColors.assign(n, Color4(1.0f, 1.0f, 1.0f, 1.0f));
VertColor *vertCol = mesh->vertColArray;
if (vertCol) {
for (int i=0; i<n; ++i) {
VertColor c = vertCol[ i ];
float a = (c.x + c.y + c.z) / 3.0f;
vertColors[i].a = a;
hasvc |= (a != 1.0f);
}
}
}
if (mesh->mapSupport(0))
{
mesh->setVCDisplayData(0);
VertColor *vertCol = mesh->vertColArray;
int n = mesh->getNumVertCol();
if (n > vertColors.size())
vertColors.assign(n, Color4(1.0f, 1.0f, 1.0f, 1.0f));
if (vertCol) {
for (int i=0; i<n; ++i) {
VertColor col = vertCol[ i ];
vertColors[i] = Color4(col.x, col.y, col.z, vertColors[i].a);
hasvc |= (col.x != 1.0f || col.y != 1.0f || col.z != 1.0f);
}
}
}
if (!hasvc) vertColors.clear();
}
#if VERSION_3DSMAX <= ((5000<<16)+(15<<8)+0) // Version 5
mesh->checkNormals(TRUE);
#else
MeshNormalSpec *specNorms = mesh->GetSpecifiedNormals ();
if (NULL != specNorms) {
specNorms->CheckNormals();
if (specNorms->GetNumNormals() == 0)
mesh->checkNormals(TRUE);
} else {
mesh->checkNormals(TRUE);
}
#endif
Result result = Ok;
Modifier* geomMorpherMod = GetMorpherModifier(node);
bool noSplit = FALSE;
// bool noSplit = (NULL != geomMorpherMod);
while (1)
{
FaceGroups grps;
if (!splitMesh(node, *mesh, grps, t, vertColors, noSplit))
{
result = Error;
break;
}
bool exportStrips = mTriStrips && (Exporter::mNifVersionInt > VER_4_2_2_0);
Matrix44 tm = Matrix44::IDENTITY;
if ( mExportExtraNodes || (mExportType != NIF_WO_ANIM && isNodeKeyed(node) ) ) {
tm = TOMATRIX4(getObjectTransform(node, t, false) * Inverse(getNodeTransform(node, t, false)));
} else {
Matrix33 rot; Vector3 trans;
objectTransform(rot, trans, node, t, local);
tm = Matrix44(trans, rot, 1.0f);
}
tm = TOMATRIX4(Inverse(mtx)) * tm;
TSTR basename = node->NodeName();
TSTR format = (!basename.isNull() && grps.size() > 1) ? "%s:%d" : "%s";
int i=1;
FaceGroups::iterator grp;
for (grp=grps.begin(); grp!=grps.end(); ++grp, ++i)
{
string name = FormatString(format, basename.data(), i);
NiTriBasedGeomRef shape = makeMesh(ninode, getMaterial(node, grp->first), grp->second, exportStrips);
if (shape == NULL)
{
result = Error;
break;
}
if (node->IsHidden())
shape->SetVisibility(false);
shape->SetName(name);
shape->SetLocalTransform(tm);
if (Exporter::mZeroTransforms) {
shape->ApplyTransforms();
}
makeSkin(shape, node, grp->second, t);
if (geomMorpherMod) {
vector<Vector3> verts = shape->GetData()->GetVertices();
exportGeomMorpherControl(geomMorpherMod, verts, shape->GetData()->GetVertexIndices(), shape);
shape->GetData()->SetConsistencyFlags(CT_VOLATILE);
}
}
break;
}
if (tri != os.obj)
tri->DeleteMe();
if (copymesh)
delete copymesh;
return result;
}
/// @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;
}
void Exporter::objectTransform(Matrix33 &rot, Vector3 &trans, INode *node, TimeValue t, bool local)
{
Matrix3 tm = getObjectTransform(node, t, local);
convertMatrix(rot, tm);
trans.Set(tm.GetTrans().x, tm.GetTrans().y, tm.GetTrans().z);
}
/// check out sawConstraintController
void updateKinematics(){
jointHandles_ = VrepRobotArmDriverP_->getJointHandles();
auto eigentestJacobian=::grl::vrep::getJacobian(*VrepRobotArmDriverP_);
/// The row/column major order is swapped between cisst and VREP!
this->currentKinematicsStateP_->Jacobian.SetSize(eigentestJacobian.cols(),eigentestJacobian.rows());
Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> > mckp2(this->currentKinematicsStateP_->Jacobian.Pointer(),this->currentKinematicsStateP_->Jacobian.cols(),this->currentKinematicsStateP_->Jacobian.rows());
mckp2 = eigentestJacobian.cast<double>();
//Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> > mf(eigentestJacobian,eigentestJacobian.cols(),eigentestJacobian.rows());
//Eigen::MatrixXf eigenJacobian = mf;
Eigen::MatrixXf eigenJacobian = eigentestJacobian;
///////////////////////////////////////////////////////////
// Copy Joint Interval, the range of motion for each joint
// lower limits
auto & llim = std::get<vrep::VrepRobotArmDriver::JointLowerPositionLimit>(currentArmState_);
std::vector<double> llimits(llim.begin(),llim.end());
jointPositionLimitsVFP_->LowerLimits = vctDoubleVec(llimits.size(),&llimits[0]);
// upper limits
auto & ulim = std::get<vrep::VrepRobotArmDriver::JointUpperPositionLimit>(currentArmState_);
std::vector<double> ulimits(ulim.begin(),ulim.end());
jointPositionLimitsVFP_->UpperLimits = vctDoubleVec(ulimits.size(),&ulimits[0]);
// current position
auto & currentJointPos = std::get<vrep::VrepRobotArmDriver::JointPosition>(currentArmState_);
std::vector<double> currentJointPosVec(currentJointPos.begin(),currentJointPos.end());
vctDoubleVec vctDoubleVecCurrentJoints(currentJointPosVec.size(),¤tJointPosVec[0]);
// update limits
/// @todo does this leak memory when called every time around?
UpdateJointPosLimitsVF(positionLimitsName,jointPositionLimitsVFP_->UpperLimits,jointPositionLimitsVFP_->LowerLimits,vctDoubleVecCurrentJoints);
const auto& handleParams = VrepRobotArmDriverP_->getVrepHandleParams();
Eigen::Affine3d currentEndEffectorPose =
getObjectTransform(
std::get<vrep::VrepRobotArmDriver::RobotTipName>(handleParams)
,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
);
auto currentEigenT = currentEndEffectorPose.translation();
auto& currentCisstT = currentKinematicsStateP_->Frame.Translation();
currentCisstT[0] = currentEigenT(0);
currentCisstT[1] = currentEigenT(1);
currentCisstT[2] = currentEigenT(2);
#ifndef IGNORE_ROTATION
Eigen::Map<Eigen::Matrix<double,3,3,Eigen::ColMajor>> ccr(currentKinematicsStateP_->Frame.Rotation().Pointer());
ccr = currentEndEffectorPose.rotation();
#endif // IGNORE_ROTATION
/// @todo set rotation component of current position
Eigen::Affine3d desiredEndEffectorPose =
getObjectTransform(
std::get<vrep::VrepRobotArmDriver::RobotTargetName>(handleParams)
,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
);
auto desiredEigenT = desiredEndEffectorPose.translation();
auto& desiredCisstT = desiredKinematicsStateP_->Frame.Translation();
desiredCisstT[0] = desiredEigenT(0);
desiredCisstT[1] = desiredEigenT(1);
desiredCisstT[2] = desiredEigenT(2);
#ifndef IGNORE_ROTATION
Eigen::Map<Eigen::Matrix<double,3,3,Eigen::ColMajor>> dcr(desiredKinematicsStateP_->Frame.Rotation().Pointer());
dcr = desiredEndEffectorPose.rotation();
#endif // IGNORE_ROTATION
/// @todo set rotation component of desired position
// for debugging, the translation between the current and desired position in cartesian coordinates
auto inputDesired_dx = desiredCisstT - currentCisstT;
vct3 dx_translation, dx_rotation;
// Rotation part
vctAxAnRot3 dxRot;
vct3 dxRotVec;
dxRot.FromNormalized((currentKinematicsStateP_->Frame.Inverse() * desiredKinematicsStateP_->Frame).Rotation());
dxRotVec = dxRot.Axis() * dxRot.Angle();
dx_rotation[0] = dxRotVec[0];
dx_rotation[1] = dxRotVec[1];
dx_rotation[2] = dxRotVec[2];
//dx_rotation.SetAll(0.0);
dx_rotation = currentKinematicsStateP_->Frame.Rotation() * dx_rotation;
Eigen::AngleAxis<float> tipToTarget_cisstToEigen;
Eigen::Matrix3f rotmat;
double theta = std::sqrt(dx_rotation[0]*dx_rotation[0]+dx_rotation[1]*dx_rotation[1]+dx_rotation[2]*dx_rotation[2]);
rotmat= Eigen::AngleAxisf(theta,Eigen::Vector3f(dx_rotation[0]/theta,dx_rotation[1]/theta,dx_rotation[2]/theta));
// std::cout << "\ntiptotarget \n" << tipToTarget.matrix() << "\n";
// std::cout << "\ntiptotargetcisst\n" << rotmat.matrix() << "\n";
//BOOST_LOG_TRIVIAL(trace) << "\n test desired dx: " << inputDesired_dx << " " << dx_rotation << "\noptimizer Calculated dx: " << optimizerCalculated_dx;
SetKinematics(*currentKinematicsStateP_); // replaced by name of object
// fill these out in the desiredKinematicsStateP_
//RotationType RotationMember; // vcRot3
//TranslationType TranslationMember; // vct3
SetKinematics(*desiredKinematicsStateP_); // replaced by name of object
// call setKinematics with the new kinematics
// sawconstraintcontroller has kinematicsState
// set the jacobian here
//////////////////////
/// @todo move code below here back under run_one updateKinematics() call
/// @todo need to provide tick time in double seconds and get from vrep API call
float simulationTimeStep = simGetSimulationTimeStep();
UpdateOptimizer(simulationTimeStep);
vctDoubleVec jointAngles_dt;
auto returncode = Solve(jointAngles_dt);
/// @todo check the return code, if it doesn't have a result, use the VREP version as a fallback and report an error.
if(returncode != nmrConstraintOptimizer::NMR_OK) BOOST_THROW_EXCEPTION(std::runtime_error("VrepInverseKinematicsController: constrained optimization error, please investigate"));
/// @todo: rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command?
std::string str;
// str = "";
for (std::size_t i=0 ; i < jointHandles_.size() ; i++)
{
float currentAngle;
auto ret = simGetJointPosition(jointHandles_[i],¤tAngle);
BOOST_VERIFY(ret!=-1);
float futureAngle = currentAngle + jointAngles_dt[i];
//simSetJointTargetVelocity(jointHandles_[i],jointAngles_dt[i]/simulationTimeStep);
//simSetJointTargetPosition(jointHandles_[i],jointAngles_dt[i]);
//simSetJointTargetPosition(jointHandles_[i],futureAngle);
simSetJointPosition(jointHandles_[i],futureAngle);
str+=boost::lexical_cast<std::string>(jointAngles_dt[i]);
if (i<jointHandles_.size()-1)
str+=", ";
}
BOOST_LOG_TRIVIAL(trace) << "jointAngles_dt: "<< str;
auto optimizerCalculated_dx = this->currentKinematicsStateP_->Jacobian * jointAngles_dt;
BOOST_LOG_TRIVIAL(trace) << "\n desired dx: " << inputDesired_dx << " " << dx_rotation << "\noptimizer Calculated dx: " << optimizerCalculated_dx;
}
