// This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages): VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData) { // This is called quite often. Just watch out for messages/events you want to handle // Keep following 5 lines at the beginning and unchanged: simLockInterface(1); int errorModeSaved; simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved); simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore); void* retVal = NULL; // Here we can intercept many messages from V-REP (actually callbacks). // Only the most important messages are listed here: if (message==sim_message_eventcallback_simulationabouttostart) { // Simulation is about to start ROS_INFO("[V-REP_DXL_ARM] Simulation is about to start."); getJointHandles(); } if (message==sim_message_eventcallback_mainscriptabouttobecalled) { std::cout << " ---- ---- ---- " << std::endl; // A simulation iteration is about to take place for (int i = 0; i < JOINT_COUNT; ++i) { simGetJointPosition(joint_handles[i], &msg.data[i]); msg.data[i] *= RAD_TO_DEG; msg.data[i] += 150; std::cout << msg.data[i] << std::endl; } msg.data[JOINT_COUNT] = 150; // Grasp -> center position pub.publish(msg); } if (message==sim_message_eventcallback_simulationended) { // Simulation just ended ROS_INFO("[V-REP_DXL_ARM] Simulation just ended."); } // Keep following unchanged: simSetIntegerParameter(sim_intparam_error_report_mode, errorModeSaved); // restore previous settings simLockInterface(0); return (retVal); }
void JointControlPluglet::velocityCallback( const brics_actuator::JointVelocities msg) { //ctl_state = Velocity; //} for (unsigned int i = 0; i < msg.velocities.size(); i++) { if (!controlledJoint(msg.velocities[i].joint_uri)) { continue; } int handle = simGetObjectHandle(msg.velocities[i].joint_uri.c_str()); if (handle > 0) { if (std::fabs(msg.velocities[i].value) < 0.0000001) { brics_actuator::JointPositions msg_single; msg_single.poisonStamp = msg.poisonStamp; brics_actuator::JointValue value = msg.velocities[i]; value.unit = "rad"; float dval = 0.0; simGetJointPosition(handles[i], &dval); value.value = dval; msg_single.positions.push_back(value); //positionCallback(msg_single); simSetObjectIntParameter(handle, 2000, 1); simSetObjectIntParameter(handle, 2001, 1); simSetJointTargetPosition(handle, dval); } else { simSetObjectIntParameter(handle, 2000, 1); simSetObjectIntParameter(handle, 2001, 0); simSetObjectIntParameter(handle, 1000, 1); simSetJointTargetVelocity(handle, static_cast<float>(msg.velocities[i].value)); } } } }
void CK3::handleSimulation() { // called when the main script calls: simHandleModule _initialize(); float dt=simGetSimulationTimeStep(); for (int i=0;i<2;i++) { if (_targetVelocities[i]>_currentVelocities[i]) { _currentVelocities[i]=_currentVelocities[i]+_maxAcceleration*dt; if (_currentVelocities[i]>_targetVelocities[i]) _currentVelocities[i]=_targetVelocities[i]; } else { _currentVelocities[i]=_currentVelocities[i]-_maxAcceleration*dt; if (_currentVelocities[i]<_targetVelocities[i]) _currentVelocities[i]=_targetVelocities[i]; } simSetJointTargetVelocity(_k3MotorHandles[i],_currentVelocities[i]); float jp; simGetJointPosition(_k3MotorHandles[i],&jp); float dp=jp-_previousMotorAngles[i]; if (fabs(dp)>piValue) dp-=(2.0f*piValue*fabs(dp)/dp); // _cumulativeMotorAngles[i]+=dp/(2.0f*piValue); _cumulativeMotorAngles[i]+=dp; // corrected on 5/3/2012 thanks to Felix Fischer _previousMotorAngles[i]=jp; } float adp=_targetArmPosition-_currentArmPosition; if (adp!=0.0f) { if (adp*_currentArmVelocity>=0.0f) { float decelToZeroTime=(fabs(_currentArmVelocity)+_maxArmAcceleration*dt*1.0f)/_maxArmAcceleration; float distToZero=0.5f*_maxArmAcceleration*decelToZeroTime*decelToZeroTime; float dir=1.0f; if (_currentArmVelocity!=0.0f) dir=_currentArmVelocity/fabs(_currentArmVelocity); else dir=adp/fabs(adp); if (fabs(adp)>distToZero) _currentArmVelocity+=dir*_maxArmAcceleration*dt; else _currentArmVelocity-=dir*_maxArmAcceleration*dt; } else _currentArmVelocity*=(1-_maxArmAcceleration*dt/fabs(_currentArmVelocity)); } else { if (_currentArmVelocity!=0.0f) { float dv=-_currentArmVelocity*_maxArmAcceleration*dt/fabs(_currentArmVelocity); if ((_currentArmVelocity+dv)*_currentArmVelocity<0.0f) _currentArmVelocity=0.0f; else _currentArmVelocity+=dv; } } _currentArmPosition+=_currentArmVelocity*dt; simSetJointTargetPosition(_k3GripperArmMotorHandles[0],_currentArmPosition); simSetJointTargetPosition(_k3GripperArmMotorHandles[1],-_currentArmPosition); simSetJointTargetPosition(_k3GripperArmMotorHandles[2],_currentArmPosition); simSetJointTargetPosition(_k3GripperArmMotorHandles[3],_currentArmPosition); simSetJointTargetPosition(_k3GripperArmMotorHandles[4],_currentArmPosition); simSetJointTargetPosition(_k3GripperArmMotorHandles[5],_currentArmPosition); float jp; simGetJointPosition(_k3GripperFingerMotorHandles[0],&jp); _currentGripperGap_unscaled=(jp/_scaling)+0.04f; // Don't forget scaling!! float dp=_targetGripperGap_unscaled-_currentGripperGap_unscaled; float velToRegulate=0.0f; if (fabs(dp)<0.005f) { if (dp!=0.0f) velToRegulate=(fabs(dp)/0.005f)*0.045f*dp/fabs(dp); } else velToRegulate=0.045f*dp/fabs(dp); simSetJointTargetVelocity(_k3GripperFingerMotorHandles[0],velToRegulate*_scaling); // Don't forget scaling!! simSetJointTargetPosition(_k3GripperFingerMotorHandles[1],-jp*0.5f/_scaling); // Don't forget scaling!! simSetJointTargetPosition(_k3GripperFingerMotorHandles[2],-jp*0.5f/_scaling); // Don't forget scaling!! _updateStateVisualization(); }
VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData) { // This is called quite often. Just watch out for messages/events you want to handle // This function should not generate any error messages: int errorModeSaved; simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved); simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore); void* retVal=NULL; if (message==sim_message_eventcallback_modulehandle) { if ( (customData==NULL)||(std::string("K3").compare((char*)customData)==0) ) // is the command also meant for Khepera3? { float dt=simGetSimulationTimeStep(); for (unsigned int k3Index=0;k3Index<allK3s.size();k3Index++) { // 1. Run the robot control: for (int i=0;i<2;i++) { if (allK3s[k3Index].targetVelocities[i]>allK3s[k3Index].currentVelocities[i]) { allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].currentVelocities[i]+allK3s[k3Index].maxAcceleration*dt; if (allK3s[k3Index].currentVelocities[i]>allK3s[k3Index].targetVelocities[i]) allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].targetVelocities[i]; } else { allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].currentVelocities[i]-allK3s[k3Index].maxAcceleration*dt; if (allK3s[k3Index].currentVelocities[i]<allK3s[k3Index].targetVelocities[i]) allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].targetVelocities[i]; } simSetJointTargetVelocity(allK3s[k3Index].wheelMotorHandles[i],allK3s[k3Index].currentVelocities[i]); float jp; simGetJointPosition(allK3s[k3Index].wheelMotorHandles[i],&jp); float dp=jp-allK3s[k3Index].previousMotorAngles[i]; if (fabs(dp)>3.1415f) dp-=(2.0f*3.1415f*fabs(dp)/dp); allK3s[k3Index].cumulativeMotorAngles[i]+=dp; // corrected on 5/3/2012 thanks to Felix Fischer allK3s[k3Index].previousMotorAngles[i]=jp; } float adp=allK3s[k3Index].targetArmPosition-allK3s[k3Index].currentArmPosition; if (adp!=0.0f) { if (adp*allK3s[k3Index].currentArmVelocity>=0.0f) { float decelToZeroTime=(fabs(allK3s[k3Index].currentArmVelocity)+allK3s[k3Index].maxArmAcceleration*dt*1.0f)/allK3s[k3Index].maxArmAcceleration; float distToZero=0.5f*allK3s[k3Index].maxArmAcceleration*decelToZeroTime*decelToZeroTime; float dir=1.0f; if (allK3s[k3Index].currentArmVelocity!=0.0f) dir=allK3s[k3Index].currentArmVelocity/fabs(allK3s[k3Index].currentArmVelocity); else dir=adp/fabs(adp); if (fabs(adp)>distToZero) allK3s[k3Index].currentArmVelocity+=dir*allK3s[k3Index].maxArmAcceleration*dt; else allK3s[k3Index].currentArmVelocity-=dir*allK3s[k3Index].maxArmAcceleration*dt; } else allK3s[k3Index].currentArmVelocity*=(1-allK3s[k3Index].maxArmAcceleration*dt/fabs(allK3s[k3Index].currentArmVelocity)); } else { if (allK3s[k3Index].currentArmVelocity!=0.0f) { float dv=-allK3s[k3Index].currentArmVelocity*allK3s[k3Index].maxArmAcceleration*dt/fabs(allK3s[k3Index].currentArmVelocity); if ((allK3s[k3Index].currentArmVelocity+dv)*allK3s[k3Index].currentArmVelocity<0.0f) allK3s[k3Index].currentArmVelocity=0.0f; else allK3s[k3Index].currentArmVelocity+=dv; } } allK3s[k3Index].currentArmPosition+=allK3s[k3Index].currentArmVelocity*dt; simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[0],allK3s[k3Index].currentArmPosition); simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[1],-allK3s[k3Index].currentArmPosition); simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[2],allK3s[k3Index].currentArmPosition); simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[3],allK3s[k3Index].currentArmPosition); simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[4],allK3s[k3Index].currentArmPosition); simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[5],allK3s[k3Index].currentArmPosition); float jp; simGetJointPosition(allK3s[k3Index].fingerMotorHandles[0],&jp); allK3s[k3Index].currentGripperGap=(jp)+0.04f; float dp=allK3s[k3Index].targetGripperGap-allK3s[k3Index].currentGripperGap; float velToRegulate=0.0f; if (fabs(dp)<0.005f) { if (dp!=0.0f) velToRegulate=(fabs(dp)/0.005f)*0.045f*dp/fabs(dp); } else velToRegulate=0.045f*dp/fabs(dp); simSetJointTargetVelocity(allK3s[k3Index].fingerMotorHandles[0],velToRegulate); simSetJointTargetPosition(allK3s[k3Index].fingerMotorHandles[1],-jp*0.5f); simSetJointTargetPosition(allK3s[k3Index].fingerMotorHandles[2],-jp*0.5f); // 2. Update the robot's UI: // IR sensors on the base: float ptAndDist[4]; std::string s; for (int i=0;i<9;i++) { if (((simGetExplicitHandling(allK3s[k3Index].irSensorHandles[i])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].irSensorHandles[i],ptAndDist,NULL,NULL)>0)) { simSetUIButtonLabel(allK3s[k3Index].uiHandle,110+i,"&&Box",NULL); std::stringstream out; out << int(ptAndDist[3]*1000.0f); s=out.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,210+i,s.c_str(),NULL); } else { simSetUIButtonLabel(allK3s[k3Index].uiHandle,110+i,"",NULL); simSetUIButtonLabel(allK3s[k3Index].uiHandle,210+i,"",NULL); } } // UI title: char* objName=simGetObjectName(allK3s[k3Index].k3BaseHandle); if (objName!=NULL) { std::string nm(objName); simReleaseBuffer(objName); nm+=" state visualization"; simSetUIButtonLabel(allK3s[k3Index].uiHandle,0,nm.c_str(),NULL); } // US sensors on the base: for (int i=0;i<5;i++) { if (((simGetExplicitHandling(allK3s[k3Index].usSensorHandles[i])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].usSensorHandles[i],ptAndDist,NULL,NULL)>0)) { simSetUIButtonLabel(allK3s[k3Index].uiHandle,100+i,"&&Box",NULL); std::stringstream out; out << int(ptAndDist[3]*1000.0f); s=out.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,200+i,s.c_str(),NULL); } else { simSetUIButtonLabel(allK3s[k3Index].uiHandle,100+i,"",NULL); simSetUIButtonLabel(allK3s[k3Index].uiHandle,200+i,"",NULL); } } // Color sensors on base: for (int i=0;i<2;i++) { float* auxValues=NULL; int* auxValuesCount=NULL; float col[3]={0.0f,0.0f,0.0f}; if (simReadVisionSensor(allK3s[k3Index].colorSensorHandles[i],&auxValues,&auxValuesCount)>=0) { if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15)) { col[0]=auxValues[11]; col[1]=auxValues[12]; col[2]=auxValues[13]; } simReleaseBuffer((char*)auxValues); simReleaseBuffer((char*)auxValuesCount); } simSetUIButtonColor(allK3s[k3Index].uiHandle,300+i,col,NULL,NULL); } // Base motor velocities and encoders: for (int i=0;i<2;i++) { std::stringstream out1,out2; out1 << std::fixed << std::setprecision(1) << allK3s[k3Index].currentVelocities[i]*180.0f/3.1415f; s=out1.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,320+i,s.c_str(),NULL); out2 << int(float(2764)*allK3s[k3Index].cumulativeMotorAngles[i]/(2.0f*3.1415f)); s=out2.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,310+i,s.c_str(),NULL); } // Arm position: std::stringstream out; out << int((1.0f-(allK3s[k3Index].currentArmPosition*180.0f/(195.0f*3.1415f)))*600.0f+300.0f); s=out.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,120,s.c_str(),NULL); simSetUISlider(allK3s[k3Index].uiHandle,121,int(1000.0f*allK3s[k3Index].currentArmPosition*180.0f/(195.0f*3.1415f))); // Finger motors: out.str(""); out << int(allK3s[k3Index].currentGripperGap*1000.0f); s=out.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,130,s.c_str(),NULL); simSetUISlider(allK3s[k3Index].uiHandle,131,int(0.5f+(1.0f-allK3s[k3Index].currentGripperGap/0.055f)*1000.0f)); simSetUISlider(allK3s[k3Index].uiHandle,132,int(0.5f+(allK3s[k3Index].currentGripperGap/0.055f)*1000.0f)); // Gripper proximity sensors: for (int i=0;i<2;i++) { if (((simGetExplicitHandling(allK3s[k3Index].gripperDistanceSensorHandles[i])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].gripperDistanceSensorHandles[i],ptAndDist,NULL,NULL)>0)) { out.str(""); out << int(ptAndDist[3]*1000.0f); s=out.str(); simSetUIButtonLabel(allK3s[k3Index].uiHandle,133+i,s.c_str(),NULL); } else simSetUIButtonLabel(allK3s[k3Index].uiHandle,133+i,"",NULL); } // Gripper color sensors: for (int i=0;i<2;i++) { float* auxValues=NULL; int* auxValuesCount=NULL; float col[3]={0.0f,0.0f,0.0f}; if (simReadVisionSensor(allK3s[k3Index].gripperColorSensorHandles[i],&auxValues,&auxValuesCount)>=0) { if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15)) { col[0]=auxValues[11]; col[1]=auxValues[12]; col[2]=auxValues[13]; } simReleaseBuffer((char*)auxValues); simReleaseBuffer((char*)auxValuesCount); } simSetUIButtonColor(allK3s[k3Index].uiHandle,135+i,col,NULL,NULL); } } } } if (message==sim_message_eventcallback_simulationended) { // simulation ended. Destroy all Khepera3 instances: allK3s.clear(); } simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings return(retVal); }
/// 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; }