! Sum the square values of the quaternion elements. @return Result of the sum of square values of the quaternion.

! Add cost term to the optimization problem. @param name Name for cost function. @param cost_term Cost term, must be an array with sh

! Random joint positions within actuator limits and optionally within a box for a given base link. @param n Number of joint positions. Defaul

! Vectorize SXContainer. @return Array containing the vectorized form for the instance.

! Add a parameter to the optimization problem. @param name Name of parameter array. @param m Number of rows in parameter array. Defau

! Get the full state trajectory for a given model. @param name Name of the model. @param time_deriv The time-deriviative required (i.

! Return the name of the model. @return Name of the model.

! Build the optimization problem.

! Return the number of elements. @return Number of elements in the SXContainer instance.

! Setup solver, note this method must return self. This is an abstract method.

! Add the equality constraint lhs == rhs to the optimization problem. @param name Name for the constraint. @param lhs Left-hand side

! Add the inequality constraint lhs <= rhs to the optimization problem. @param name Name for the constraint. @param lhs Left-hand sid

! Return the quaternion vector. @return The quaternion as an array.

! Get the link position in the global frame for a given joint state q. @param link Name of the end-effector link. @param q Joint posi

! Automate function generation. This is an internal function for the RobotModel class and <b>SHOULD NOT</b> be used.

! Reset the parameters for the optimization problem. @param p Specifies the parameters.

! Get the model state at a given time. @param name Name of the model. @param t Index of the desired state. @param time_deriv

! Fix configuration. @param name Name of model. @param config The configuration. When None is passed then it is considered to be zero

! The root link name. @return The name of the root link.

! Interpolate a trajectory @param traj The trajectory to be interpolated where the columns correspond to the states over time. @param

! Quaternion inverse. @return Inverse quaternion.

! Create skew-symmetric matrix. If V (1x1) then (order 2) S = | 0 -v | | v 0 | and if V (1x3) then (order 3) S =

! Add the inequality constraint lhs <= mid <= rhs to the optimization problem. @param name Name for the constraint. @param lhs Left-h

! Enforce model limits. @param name Name of model. @param time_deriv The time-deriviative required (i.e. position is 0, velocity is 1

! Integrates the model states over time. @param name Name of the model. @param time_deriv The time-deriviative required (i.e. positio

! Reset initial seed for the optimization problem. @param x0 The initial seed.

! 3-by-3 identity matrix. @return Identity matrix of order 3.

! Get the link transform in the global frame for a given joint state q. @param link Name of the end-effector link. @param q Joint pos

! Add the inequality constraint lhs >= rhs to the optimization problem. @param name Name for the constraint. @param lhs Left-hand sid

! Internal method.

! Get the axis of a joint. @param The joint of interest. @return The normalized joint axis.

! Roll-pitch-yaw angles to SO(3) rotation matrix. @param rpy Roll-Pitch-Yaw angles in radians. @param opt Order option. Acceptable inputs:

! Vectorize dictionary with same layout as container. @param d Dictionary containing values to be vectorized. @return Vectorized form

! Compute the geometric Jacobian matrix in the global frame. @param link Name of the end-effector link. @param q Joint position array

! Get the link rotation in the global frame for a given joint state q. @param link Name of the end-effector link. @param q Joint posi

! Convert rotation and translation to homogeneous transform. @param R A 3-by-3 rotation matrix. @param t A vector with 3 elements. @retur

! Return the decision variables as a casadi.SX vector. @return Symbolic decision variables.

! Get the function that computes the global position of a given link. @param link Name of the end-effector link. @param n Number of j

! Return the model with given name. @param name Name of the model. @return A task or robot model.

! Set initial configuration. @param name Name of model. @param init Initial configuration. If None is passed then this is assumed to

! Split the quaternion into its xyzw parts. @return The xyzw parts of the quaternion.

! Return the parameter name. @param time_deriv The time-deriviative required (i.e. position is 0, velocity is 1, etc.) @return The pa

! Initializer for the Optimization class. @param decision_variables SXContainer containing decision variables. @param parameters SXCo

! Get the joint index for a given joint name. @param The name of the joint. @return Index for the joint.

! Get a quaternion in the global frame. @param link Name of the end-effector link. @param q Joint position array. @return Qua

! Draw a line. @param start The start of the line. @param end The end of the line. @param rgb The Red-Green-Blue values in ra

! Set the RGB and alpha channels for an actor. @param actor A vtk actor object. @param rgb The Red-Green-Blue values in range [0, 1].

! Draw a grid floor. @param num_cells The number of cells for the grid floor. @param rgb The Red-Green-Blue values in range [0, 1].

! Draw a sphere. @param radius The radius of the sphere. @param position The position of the sphere. @param rgb The Red-Green

! Return the state name. @param time_deriv The time-deriviative required (i.e. position is 0, velocity is 1, etc.) @return The state

! Internal method.

! Add decision variables to the optimization problem. @param name Name of decision variable array. @param m Number of rows in decisio

! Convert angle and vector orientation to a rotation matrix. This method uses Rodrigue's formula. @param theta Angle of rotation (radians). @

! Return a quaternion from Roll-Pitch-Yaw angles. @param rpy Roll-Pitch-Yaw angles (radians). @return An instance of the Quaternion c

! Get the function that computes a quaternion in the global frame. @param link Name of the end-effector link. @param n Number of join

! Get the origin for the joint. @param The joint of interest. @return The position and orientation of the joint.

! Get the geometric jacobian in a given base link. @param link Name of the end-effector link. @param q Joint position array.

! Return the quaternion as Roll-Pitch-Yaw angles. @return The RPY Euler angles (in radians).

! Solve the optimization problem. @return A dictionary containing the solution.

! SE(3) translational homogeneous transform. @param T Homogeneous transformation matrix. @return Translation part of the homogeneous transfor

! Compute the Jacobian and Hessian for a given function using automatic differentiation. @param name The function name. @param fun The CasADi

! Returns true when the solver solved the previous problem, false otherwise. This is an abstract method. @return Result of whether the solver

! Return a quaternion from angle-vector form. @param theta Angle of rotation (radians). @param v Direction vector to rotate about.

! Return a quaternion from a quaternion vector array. @param q Quaternion as an array. @return An instance of the Quaternion class.

! Compute the linear part of the geometric jacobian in the global frame. @param link Name of the end-effector link. @param q Joint po

! Return the lower limit for a given joint. @param joint The joint instance from the URDF. @return The lower limit, when undefined th

! Return the upper limit for a given joint. @param joint The joint instance from the URDF. @return The upper limit, when undefined th

! Return the model limits. @param time_deriv The time-deriviative required (i.e. position is 0, velocity is 1, etc.) @return lower Th

! Get the link transform in a given base frame. @param link Name of the end-effector link. @param q Joint position array. @pa