Create the Robot

robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);

Set Up the Simulation

Set the timespan to be 1 s with a timestep size of 0.01 s. Set the initial state to be the robots, home configuration with a velocity of zero.

tspan = 0:0.01:1;
initialState = [homeConfiguration(robot); zeros(7,1)];

Define the a reference state with a target position, zero velocity, and zero acceleration.

targetState = [pi/4; pi/3; pi/2; -pi/3; pi/4; -pi/4; 3*pi/4; zeros(7,1); zeros(7,1)];

Create the Motion Model

Model the system with computed torque control and error dynamics defined by a moderately fast step response with 5% overshoot.

motionModel = jointSpaceMotionModel("RigidBodyTree",robot);
updateErrorDynamicsFromStep(motionModel,.3,.05);

Simulate the Robot

Use the derivative function of the model as the input to the ode45 solver to simulate the behavior over 1 second.

[t,robotState] = ode45(@(t,state)derivative(motionModel,state,targetState),tspan,initialState);

Plot the Response

Plot the positions of all the joints actuating to their target state. Joints with a higher displacement between the starting position and the target position actuate to the target at a faster rate than those with a lower displacement. This leads to an overshoot, but all of the joints have the same settling time.

figure
plot(t,robotState(:,1:motionModel.NumJoints));
hold all;
plot(t,targetState(1:motionModel.NumJoints)*ones(1,length(t)),"--");
title("Joint Position (Solid) vs Reference (Dashed)");
xlabel("Time (s)")
ylabel("Position (rad)");

Create the Robot

robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);

Set Up the Simulation

Set the time span to be 1 second with a timestep size of 0.02 seconds. Set the initial state to the home configuration of the robot, with a velocity of zero.

tspan = 0:0.02:1;
initialState = [homeConfiguration(robot);zeros(7,1)];

Define a reference state with a target position and zero velocity.

refPose = trvec2tform([0.6 -.1 0.5]);
refVel = zeros(6,1);

Create the Motion Model

Model the behavior as a system under proportional-derivative (PD) control.

motionModel = taskSpaceMotionModel("RigidBodyTree",robot,"EndEffectorName","EndEffector_Link");

Simulate the Robot

Simulate the behavior over 1 second using a stiff solver to more efficiently capture the robot dynamics. Using ode15s enables higher precision around the areas with a high rate of change.

[t,robotState] = ode15s(@(t,state)derivative(motionModel,state,refPose,refVel),tspan,initialState);

Plot the Response

Plot the robot's initial position and mark the target with an X.

figure
show(robot,initialState(1:7));
hold all
plot3(refPose(1,4),refPose(2,4),refPose(3,4),"x","MarkerSize",20)

Observe the response by plotting the robot in a 5 Hz loop.

r = rateControl(5);
for i = 1:size(robotState,1)
    show(robot,robotState(i,1:7)',"PreservePlot",false);
    waitfor(r);
end