void CSubscriberData::setJointPositionCallback(const std_msgs::Float64::ConstPtr& pos) { if (_handleGeneralCallback_before()) { if (simSetJointPosition(auxInt1,(float)pos->data)==-1) shutDownGeneralSubscriber(); _handleGeneralCallback_after(); } }
void CSubscriberData::setJointStateCallback(const vrep_common::JointSetStateData::ConstPtr& data) { if ( (data->handles.data.size()>0)&&(data->handles.data.size()==data->setModes.data.size())&&(data->handles.data.size()==data->values.data.size()) ) { for (unsigned int i=0;i<data->handles.data.size();i++) { int handle=data->handles.data[i]; unsigned char setMode=data->setModes.data[i]; float val=data->values.data[i]; if (setMode==0) simSetJointPosition(handle,val); if (setMode==1) simSetJointTargetPosition(handle,val); if (setMode==2) simSetJointTargetVelocity(handle,val); if (setMode==3) simSetJointForce(handle,val); } } }
/// 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; }