Plan and Execute Task- and Joint-space Trajectories using KINOVA Gen3 Manipulator

This example shows how to generate and simulate interpolated joint trajectories to move from an initial to a desired end-effector pose. The timing of the trajectories is based on an approximate desired end of arm tool (EOAT) speed.

Load the KINOVA Gen3 rigid body tree (RBT) robot model.

`robot = loadrobot('kinovaGen3','DataFormat','row','Gravity',[0 0 -9.81]);`

Set current robot joint configuration.

`currentRobotJConfig = homeConfiguration(robot);`

Get number of joints and the end-effector RBT frame.

```numJoints = numel(currentRobotJConfig); endEffector = "EndEffector_Link";```

Specify the trajectory time step and approximate desired tool speed.

```timeStep = 0.1; % seconds toolSpeed = 0.1; % m/s```

Set the initial and final end-effector pose.

```jointInit = currentRobotJConfig; taskInit = getTransform(robot,jointInit,endEffector); taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);```

Compute task-space trajectory waypoints via interpolation.

First, compute tool traveling distance.

`distance = norm(tform2trvec(taskInit)-tform2trvec(taskFinal));`

Next, define trajectory times based on traveling distance and desired tool speed.

```initTime = 0; finalTime = (distance/toolSpeed) - initTime; trajTimes = initTime:timeStep:finalTime; timeInterval = [trajTimes(1); trajTimes(end)];```

Interpolate between `taskInit` and `taskFinal` to compute intermediate task-space waypoints.

`[taskWaypoints,taskVelocities] = transformtraj(taskInit,taskFinal,timeInterval,trajTimes); `

Create a joint space motion model for PD control on the joints. The `taskSpaceMotionModel` object models the motion of a rigid body tree model under task-space proportional-derivative control.

`tsMotionModel = taskSpaceMotionModel('RigidBodyTree',robot,'EndEffectorName','EndEffector_Link');`

Set the proportional and derivative gains on orientation to zero, so that controlled behavior just follows the reference positions:

```tsMotionModel.Kp(1:3,1:3) = 0; tsMotionModel.Kd(1:3,1:3) = 0;```

Define the initial states (joint positions and velocities).

```q0 = currentRobotJConfig; qd0 = zeros(size(q0));```

Use `ode15s` to simulate the robot motion. Since reference state changes at each instant, a wrapper function is required to update the interpolated trajectory input to the state derivative at each instant. Therefore, an example helper function is passed as the function handle to the ODE solver. The resultant manipulator states are output in `stateTask`.

`[tTask,stateTask] = ode15s(@(t,state) exampleHelperTimeBasedTaskInputs(tsMotionModel,timeInterval,taskInit,taskFinal,t,state),timeInterval,[q0; qd0]);`

Generate Joint-Space Trajectory

Create a inverse kinematics object for the robot.

```ik = inverseKinematics('RigidBodyTree',robot); ik.SolverParameters.AllowRandomRestart = false; weights = [1 1 1 1 1 1];```

Calculate the initial and desired joint configurations using inverse kinematics.

```initialGuess = wrapToPi(jointInit); jointFinal = ik(endEffector,taskFinal,weights,initialGuess); jointFinal = wrapToPi(jointFinal);```

Interpolate between them using a cubic polynomial function to generate an array of evenly-spaced joint configurations. Use a B-spline to generate a smooth trajectory.

```ctrlpoints = [jointInit',jointFinal']; jointConfigArray = cubicpolytraj(ctrlpoints,timeInterval,trajTimes); jointWaypoints = bsplinepolytraj(jointConfigArray,timeInterval,1);```

Control Joint-Space Trajectory

Create a joint space motion model for PD control on the joints. The `jointSpaceMotionModel` object models the motion of a rigid body tree model and uses proportional-derivative control on the specified joint positions.

`jsMotionModel = jointSpaceMotionModel('RigidBodyTree',robot,'MotionType','PDControl');`

Set initial states (joint positions and velocities).

```q0 = currentRobotJConfig; qd0 = zeros(size(q0));```

Use `ode15s` to simulate the robot motion. Again, an example helper function is used as the function handle input to the ODE solver in order to update the reference inputs at each instant in time. The joint-space states are output in `stateJoint`.

`[tJoint,stateJoint] = ode15s(@(t,state) exampleHelperTimeBasedJointInputs(jsMotionModel,timeInterval,jointConfigArray,t,state),timeInterval,[q0; qd0]);`

Visualize and Compare Robot Trajectories

Show the initial configuration of the robot.

```show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off'); hold on axis([-1 1 -1 1 -0.1 1.5]);```

Visualize the task-space trajectory. Iterate through the `stateTask` states and interpolate based on the current time.

```for i=1:length(trajTimes) % Current time tNow= trajTimes(i); % Interpolate simulated joint positions to get configuration at current time configNow = interp1(tTask,stateTask(:,1:numJoints),tNow); poseNow = getTransform(robot,configNow,endEffector); show(robot,configNow,'PreservePlot',false,'Frames','off'); plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'b.','MarkerSize',20) drawnow; end```

Visualize the joint-space trajectory. Iterate through the `jointTask` states and interpolate based on the current time.

```% Return to initial configuration show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off'); for i=1:length(trajTimes) % Current time tNow= trajTimes(i); % Interpolate simulated joint positions to get configuration at current time configNow = interp1(tJoint,stateJoint(:,1:numJoints),tNow); poseNow = getTransform(robot,configNow,endEffector); show(robot,configNow,'PreservePlot',false,'Frames','off'); plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'r.','MarkerSize',20) drawnow; end```

평가판 신청