void Direct3D::UpdateCamera() //Move the 3D camera, not needed in 2D. { D3DXVECTOR3 EyePos(0, 0, 0); D3DXVECTOR3 TargetPos(0, 0, 0); D3DXVECTOR3 UpVector(0, 0, 0); D3DXMATRIXA16 View; D3DXMatrixLookAtLH(&View, &EyePos, &TargetPos, &UpVector); D3DDevice->SetTransform(D3DTS_VIEW, &View); }
RobotInterface::Status Test_IK_QP::RobotUpdate(){ //updates the kinematic chain mSKinematicChain->setJoints(mJointKinematics.Array()); mSKinematicChain->getEndPos(lPos.Array()); mSKinematicChain->getJoints(mJointDesPos.Array()); //gets the jacobian from the kinematic chain and gives it to the solver Matrix tmpJ(6,KUKA_DOF); mSKinematicChain->getJacobian(tmpJ); mSolver.setJacobian(tmpJ); Vector TargetPos(6); if(mStep==0){ //initial position of the end effector mInitPos = lPos; mStep = 1; mTIME=0; } if(mStep == 1){ //The end effector goes to the desired position to follow the trajectory double TimeToInitialPosition(10); //Target position of the end effector TargetPos[0] = (1-mTIME/TimeToInitialPosition)* mInitPos[0] ; TargetPos[1] = (1-mTIME/TimeToInitialPosition)* mInitPos[1] + 0.5* mTIME/TimeToInitialPosition ; TargetPos[2] = (1-mTIME/TimeToInitialPosition)* mInitPos[2] + 0.6* mTIME/TimeToInitialPosition ; //Velocity of the end effector if the trajectory is perfectly followed, this is the (if possible analytical) time derivative of TargetPos mTargetVelocity[0] = (- mInitPos[0] )/TimeToInitialPosition ; mTargetVelocity[1] = (- mInitPos[1] + 0.5)/TimeToInitialPosition ; mTargetVelocity[2] = (- mInitPos[2] + 0.6)/TimeToInitialPosition ; if(mTIME>TimeToInitialPosition){ mTIME=0; mStep = 2; } } if(mStep ==2){ //"infinity motion //Proportional to the frequency of the loop double k = 0.4; TargetPos[0] = 0.5 *sin(sin(k*mTIME) * sin(k*mTIME)); TargetPos[1] = 0.5* cos(sin(k*mTIME) * sin(k*mTIME)); TargetPos[2] = 0.6 + 0.2 * sin(4*k*mTIME); //the target velocity has been found with wolfram, numerical differentiation of TargetPos would also work mTargetVelocity[0] = 0.5*k*sin(2*k*mTIME) * cos(sin(k*mTIME)*sin(k*mTIME)); mTargetVelocity[1] = -0.5*2*k*sin(k*mTIME) * sin(sin(k*mTIME)*sin(k*mTIME))*cos(k* mTIME); mTargetVelocity[2] = 4* 0.2 * k* cos(4*k*mTIME); } Vector tmpRdot(6); //Velocity of the end effector that will be given to the solver, it includes a gain to do positional tracking in addition to velocity tracking double gain (0.2); for(unsigned int i(0); i<3; ++i){ tmpRdot[i] = mTargetVelocity[i]+ gain*(TargetPos[i]- lPos[i])/_dt; } //Solves the inverse kinematics mSolver.Solve(tmpRdot, mSensorsGroup.GetJointAngles()); //gets the joints velocity mJointDesVel = mSolver.getOutput(); //update the position of the joints by forward Euler numerical integration mSKinematicChain->getJoints(lJoints.Array()); mJointDesPos = lJoints + mJointDesVel*_dt; //sets the updated joints positions mSKinematicChain->setJoints(mJointDesPos.Array()); mJointKinematics.Set(mJointDesPos); mTIME += _dt; return STATUS_OK; }