本文整理汇总了C++中Vector6d::resize方法的典型用法代码示例。如果您正苦于以下问题:C++ Vector6d::resize方法的具体用法?C++ Vector6d::resize怎么用?C++ Vector6d::resize使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Vector6d
的用法示例。
在下文中一共展示了Vector6d::resize方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: motionSubspaceDotTimesV
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);
}
}