C++ Vector6d::rows方法代码示例

void RollPitchYawFloatingJoint::motionSubspaceDotTimesV(const Eigen::Ref<const VectorXd>& q, const Eigen::Ref<const VectorXd>& v,
    Vector6d& motion_subspace_dot_times_v,
    Gradient<Vector6d, Eigen::Dynamic>::type* dmotion_subspace_dot_times_vdq,
    Gradient<Vector6d, Eigen::Dynamic>::type* dmotion_subspace_dot_times_vdv) const
{
  motion_subspace_dot_times_v.resize(TWIST_SIZE, 1);
  auto rpy = q.middleRows<RPY_SIZE>(SPACE_DIMENSION);
  double roll = rpy(0);
  double pitch = rpy(1);
  double yaw = rpy(2);

  auto pd = v.middleRows<SPACE_DIMENSION>(0);
  double xd = pd(0);
  double yd = pd(1);
  double zd = pd(2);

  auto rpyd = v.middleRows<RPY_SIZE>(SPACE_DIMENSION);
  double rolld = rpyd(0);
  double pitchd = rpyd(1);
  double yawd = rpyd(2);

  using namespace std;
  double cr = cos(roll);
  double sr = sin(roll);
  double cp = cos(pitch);
  double sp = sin(pitch);
  double cy = cos(yaw);
  double sy = sin(yaw);

  motion_subspace_dot_times_v.transpose() << -pitchd * yawd * cp, rolld * yawd * cp * cr - pitchd * yawd * sp * sr - pitchd * rolld * sr, -pitchd * rolld * cr - pitchd * yawd * cr * sp - rolld * yawd * cp * sr, yd * (yawd * cp * cy - pitchd * sp * sy) - xd * (pitchd * cy * sp + yawd * cp * sy)
      - pitchd * zd * cp, zd * (rolld * cp * cr - pitchd * sp * sr) + xd * (rolld * (sr * sy + cr * cy * sp) - yawd * (cr * cy + sp * sr * sy) + pitchd * cp * cy * sr) - yd * (rolld * (cy * sr - cr * sp * sy) + yawd * (cr * sy - cy * sp * sr) - pitchd * cp * sr * sy), xd
      * (rolld * (cr * sy - cy * sp * sr) + yawd * (cy * sr - cr * sp * sy) + pitchd * cp * cr * cy) - zd * (pitchd * cr * sp + rolld * cp * sr) + yd * (yawd * (sr * sy + cr * cy * sp) - rolld * (cr * cy + sp * sr * sy) + pitchd * cp * cr * sy);

  if (dmotion_subspace_dot_times_vdq) {
    dmotion_subspace_dot_times_vdq->resize(motion_subspace_dot_times_v.rows(), getNumPositions());
    dmotion_subspace_dot_times_vdq->transpose() << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -pitchd * rolld * cr - pitchd * yawd * cr * sp - rolld * yawd * cp * sr, pitchd * rolld * sr + pitchd * yawd * sp * sr - rolld * yawd * cp * cr, 0.0, xd
        * (rolld * (cr * sy - cy * sp * sr) + yawd * (cy * sr - cr * sp * sy) + pitchd * cp * cr * cy) - zd * (pitchd * cr * sp + rolld * cp * sr) + yd * (-rolld * (cr * cy + sp * sr * sy) + yawd * (sr * sy + cr * cy * sp) + pitchd * cp * cr * sy), -zd * (rolld * cp * cr - pitchd * sp * sr)
        - xd * (rolld * (sr * sy + cr * cy * sp) - yawd * (cr * cy + sp * sr * sy) + pitchd * cp * cy * sr) + yd * (rolld * (cy * sr - cr * sp * sy) + yawd * (cr * sy - cy * sp * sr) - pitchd * cp * sr * sy), pitchd * yawd * sp, -pitchd * yawd * cp * sr - rolld * yawd * cr * sp, rolld * yawd * sp
        * sr - pitchd * yawd * cp * cr, -xd * (pitchd * cp * cy - yawd * sp * sy) - yd * (pitchd * cp * sy + yawd * cy * sp) + pitchd * zd * sp, -zd * (pitchd * cp * sr + rolld * cr * sp) - xd * (-rolld * cp * cr * cy + pitchd * cy * sp * sr + yawd * cp * sr * sy)
        + yd * (rolld * cp * cr * sy + yawd * cp * cy * sr - pitchd * sp * sr * sy), -zd * (pitchd * cp * cr - rolld * sp * sr) - xd * (pitchd * cr * cy * sp + rolld * cp * cy * sr + yawd * cp * cr * sy) - yd * (-yawd * cp * cr * cy + pitchd * cr * sp * sy + rolld * cp * sr * sy), 0.0, 0.0, 0.0, -xd
        * (yawd * cp * cy - pitchd * sp * sy) - yd * (pitchd * cy * sp + yawd * cp * sy), yd * (rolld * (sr * sy + cr * cy * sp) - yawd * (cr * cy + sp * sr * sy) + pitchd * cp * cy * sr) + xd * (rolld * (cy * sr - cr * sp * sy) + yawd * (cr * sy - cy * sp * sr) - pitchd * cp * sr * sy), yd
        * (rolld * (cr * sy - cy * sp * sr) + yawd * (cy * sr - cr * sp * sy) + pitchd * cp * cr * cy) - xd * (-rolld * (cr * cy + sp * sr * sy) + yawd * (sr * sy + cr * cy * sp) + pitchd * cp * cr * sy);
  }

  if (dmotion_subspace_dot_times_vdv) {
    dmotion_subspace_dot_times_vdv->resize(motion_subspace_dot_times_v.rows(), getNumVelocities());
    dmotion_subspace_dot_times_vdv->transpose() << 0.0, 0.0, 0.0, -pitchd * cy * sp - yawd * cp * sy, rolld * (sr * sy + cr * cy * sp) - yawd * (cr * cy + sp * sr * sy) + pitchd * cp * cy * sr, rolld * (cr * sy - cy * sp * sr) + yawd * (cy * sr - cr * sp * sy) + pitchd * cp * cr * cy, 0.0, 0.0, 0.0, yawd * cp
        * cy - pitchd * sp * sy, -rolld * (cy * sr - cr * sp * sy) - yawd * (cr * sy - cy * sp * sr) + pitchd * cp * sr * sy, -rolld * (cr * cy + sp * sr * sy) + yawd * (sr * sy + cr * cy * sp) + pitchd * cp * cr * sy, 0.0, 0.0, 0.0, -pitchd * cp, rolld * cp * cr - pitchd * sp * sr, -pitchd * cr
        * sp - rolld * cp * sr, 0.0, -pitchd * sr + yawd * cp * cr, -pitchd * cr - yawd * cp * sr, 0.0, xd * (sr * sy + cr * cy * sp) - yd * (cy * sr - cr * sp * sy) + zd * cp * cr, xd * (cr * sy - cy * sp * sr) - yd * (cr * cy + sp * sr * sy) - zd * cp * sr, -yawd * cp, -sr * (rolld + yawd * sp), -cr
        * (rolld + yawd * sp), -zd * cp - xd * cy * sp - yd * sp * sy, sr * (-zd * sp + xd * cp * cy + yd * cp * sy), cr * (-zd * sp + xd * cp * cy + yd * cp * sy), -pitchd * cp, rolld * cp * cr - pitchd * sp * sr, -pitchd * cr * sp - rolld * cp * sr, cp * (yd * cy - xd * sy), -xd
        * (cr * cy + sp * sr * sy) - yd * (cr * sy - cy * sp * sr), xd * (cy * sr - cr * sp * sy) + yd * (sr * sy + cr * cy * sp);
  }
}

/*!
 * This function is implemented in order to not use any external
 * mapping functions. The function was build completely from pieces of
 * code of the old cartesianStateDerivativeModel in order to use
 * completely different, but also verified methods of constructing a
 * stateDerivative from the total accelerations.
 * \param currentState The current state.
 * \param accelerations Total sum of accelerations.
 * \return stateDerivative
 */
Vector6d mapCartesian( const Vector6d& cartesianState, const Eigen::Vector3d& acceleration )
{
    
    // {{{ Snippets from cartesianStateDerivativeModel.h
    typedef Vector6d CartesianStateDerivativeType;

    // Declare Cartesian state derivative size.
    unsigned int stateDerivativeSize = cartesianState.rows( );

    // Declare Cartesian state derivative of the same size as Cartesian state.
    CartesianStateDerivativeType cartesianStateDerivative
	= CartesianStateDerivativeType::Zero( stateDerivativeSize );

    // Set derivative of position components to current Cartesian velocity.
    cartesianStateDerivative.segment( 0, stateDerivativeSize / 2 )
	= cartesianState.segment( stateDerivativeSize / 2, stateDerivativeSize / 2 );

    // Add transformed acceleration to state derivative.
    cartesianStateDerivative.segment( stateDerivativeSize / 2, stateDerivativeSize / 2 )
	+= acceleration;

    // Return assembled state derivative.
    return cartesianStateDerivative;

    // }}} End Snippets from cartesianStateDerivativeModel.h
}