# derivative

Time derivative of manipulator model states

## Syntax

``stateDot = derivative(taskMotionModel,state,refPose,refVel)``
``stateDot = derivative(taskMotionModel,state,refPose, refVel,fExt)``
``stateDot = derivative(jointMotionModel,state,cmds) ``
``stateDot = derivative(jointMotionModel,state,cmds,fExt)``

## Description

example

````stateDot = derivative(taskMotionModel,state,refPose,refVel)` computes the time derivative of the motion model based on the current state and motion commands using a task-space model.```
````stateDot = derivative(taskMotionModel,state,refPose, refVel,fExt)` computes the time derivative based on the current state, motion commands, and any external forces on the manipulator using a task space model.```

example

````stateDot = derivative(jointMotionModel,state,cmds) ` computes the time derivative of the motion model based on the current state and motion commands using a joint-space model.```
````stateDot = derivative(jointMotionModel,state,cmds,fExt)` computes the time derivative based on the current state, motion commands, and any external forces on the manipulator using a joint-space model.```

## Examples

collapse all

This example shows how to create and use a `jointSpaceMotionModel` object for a manipulator robot in joint-space.

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)");``` This example shows how to create and use a `taskSpaceMotionModel` object for a manipulator robot arm in task-space.

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``` ## Input Arguments

collapse all

`taskSpaceMotionModel` object, which defines the properties of the motion model.

`jointSpaceMotionModel` object, which defines the properties of the motion model.

Joint positions and velocities represented as a 2n-element vector, specified as [q; qDot]. n is the number of non-fixed joints in the associated `rigidBodyTree` of the `motionModel`. q, represents the position of each joint, specified in radians. qDot represents the velocity of each joint, specified in radians per second.

The reference pose of the end effector in the task-space in meters, specified as an 4-by-4 homogeneous transformation matrix.

The reference velocities of the end effector in the task space, specified as a six-element vector of real values, specified as [omega v]. omega represents a row vector of three angular velocities about the `x`, `y`, and `z` axes, specified in radians per second, and v represents a row vector of three linear velocities along the `x`, `y`, and `z` axes, specified in meters per second.

Control commands indicating desired motion. The dimensions of `cmds` depend on the MotionType property of the motion model:

• `"PDControl"` — 2-by-n matrix, `[qRef; qRefDot]`. The first and second rows represent joint positions and joint velocities, respectively.

• `"ComputedTorqueControl"` — 3-by-n matrix, `[qRef; qRefDot; qRefDDot]`. The first, second, and third rows represent joint positions, joint velocities, and joint accelerations respectively.

• `"IndependentJointMotion"` — 3-by-n matrix, `[qRef; qRefDot; qRefDDot]`. The first, second, and third rows represent joint positions, joint velocities, and joint accelerations respectively.

Note that `jointSpaceMotionModel` supports all three MotionType listed above, but `taskSpaceMotionModel` only supports `"PDControl"` MotionType.

External forces, specified as an m-element vector, where m is the number of bodies in the associated `rigidBodyTree` object.

## Output Arguments

collapse all

Time derivative based on current state and specified control commands, returned as a 2-by-n matrix of real values, [qDot; qDDot], where qDot is an n-element row vector of joint velocities, and qDDot is an n-element row vector of joint accelerations. n is the number of joints in the associated `rigidBodyTree` of the `motionModel`.