コード例 #1
0
ファイル: Pose2MobileVetLinArm.cpp プロジェクト: gtrll/gpmp2
/* ************************************************************************** */
void Pose2MobileVetLinArm::forwardKinematics(const Pose2Vector& p, 
    boost::optional<const gtsam::Vector&> v,
    std::vector<gtsam::Pose3>& px, boost::optional<std::vector<gtsam::Vector3>&> vx,
    boost::optional<std::vector<gtsam::Matrix>&> J_px_p,
    boost::optional<std::vector<gtsam::Matrix>&> J_vx_p,
    boost::optional<std::vector<gtsam::Matrix>&> J_vx_v) const {
    
  if (v)
    throw runtime_error("[Pose2MobileArm] TODO: velocity not implemented");

  if (!v && (vx || J_vx_p || J_vx_v))
    throw runtime_error("[Pose2MobileArm] ERROR: only ask for velocity in workspace given velocity in "
        "configuration space");

  // space for output
  px.resize(nr_links());
  if (vx) vx->resize(nr_links());
  if (J_px_p) J_px_p->assign(nr_links(), Matrix::Zero(6, dof()));
  if (J_vx_p) J_vx_p->assign(nr_links(), Matrix::Zero(3, dof()));
  if (J_vx_v) J_vx_v->assign(nr_links(), Matrix::Zero(3, dof()));

  // vehicle & arm base pose
  Pose3 veh_base, tso_base, arm_base;
  Matrix63 Hveh_base;
  Matrix64 Htso_base, Harm_base;
  if (J_px_p || J_vx_p || J_vx_v) {
    veh_base = computeBasePose3(p.pose(), Hveh_base);
    tso_base = liftBasePose3(p.pose(), p.configuration()(0), base_T_torso_, reverse_linact_, 
        Htso_base);
    Matrix6 H_tso_comp;
    arm_base = tso_base.compose(torso_T_arm_, H_tso_comp);
    Harm_base = H_tso_comp * Htso_base;

  } else {
    veh_base = computeBasePose3(p.pose());
    tso_base = liftBasePose3(p.pose(), p.configuration()(0), base_T_torso_, reverse_linact_);
    arm_base = tso_base.compose(torso_T_arm_);
  }

  // veh base link
  px[0] = veh_base;
  if (J_px_p) (*J_px_p)[0].block<6,3>(0,0) = Hveh_base;

  // torso link
  px[1] = tso_base;
  if (J_px_p) (*J_px_p)[1].block<6,4>(0,0) = Htso_base;

  // arm links
  vector<Pose3> armjpx;
  vector<Matrix> Jarm_jpx_jp;

  arm_.updateBasePose(arm_base);
  arm_.forwardKinematics(p.configuration().tail(arm_.dof()), boost::none, armjpx, boost::none,
      J_px_p ? boost::optional<vector<Matrix>&>(Jarm_jpx_jp) : boost::none);

  for (size_t i = 0; i < arm_.dof(); i++) {
    px[i+2] = armjpx[i];
    if (J_px_p) {
      // see compose's jacobian
      (*J_px_p)[i+2].block<6,4>(0,0) = (armjpx[i].inverse() * arm_base).AdjointMap() * Harm_base;
      (*J_px_p)[i+2].block(0,4,6,arm_.dof()) = Jarm_jpx_jp[i];
    }
  }
}
コード例 #2
0
ファイル: Pose2MobileArm.cpp プロジェクト: gtrll/gpmp2
/* ************************************************************************** */
void Pose2MobileArm::forwardKinematics(
    const Pose2Vector& p, boost::optional<const gtsam::Vector&> v,
    std::vector<gtsam::Pose3>& px, boost::optional<std::vector<gtsam::Vector3>&> vx,
    boost::optional<std::vector<gtsam::Matrix>&> J_px_p,
    boost::optional<std::vector<gtsam::Matrix>&> J_vx_p,
    boost::optional<std::vector<gtsam::Matrix>&> J_vx_v) const {

  if (v)
    throw runtime_error("[Pose2MobileArm] TODO: velocity not implemented");

  if (!v && (vx || J_vx_p || J_vx_v))
    throw runtime_error("[Pose2MobileArm] ERROR: only ask for velocity in workspace given velocity in "
        "configuration space");

  // space for output
  px.resize(nr_links());
  if (vx) vx->resize(nr_links());
  if (J_px_p) J_px_p->assign(nr_links(), Matrix::Zero(6, dof()));
  if (J_vx_p) J_vx_p->assign(nr_links(), Matrix::Zero(3, dof()));
  if (J_vx_v) J_vx_v->assign(nr_links(), Matrix::Zero(3, dof()));

  // vehicle & arm base pose
  Pose3 veh_base, arm_base;
  Matrix63 Hveh_base, Harm_base;
  if (J_px_p || J_vx_p || J_vx_v) {
    veh_base = computeBasePose3(p.pose(), Hveh_base);
    arm_base = computeBaseTransPose3(p.pose(), base_T_arm_, Harm_base);
  } else {
    veh_base = computeBasePose3(p.pose());
    arm_base = computeBaseTransPose3(p.pose(), base_T_arm_);
  }


  // call arm pose and velocity, for arm links
  // px[0] = base_pose3; px[i] = arm_base * px_arm[i-1]
  // vx[0] = v(0:1,0); vx[i] = vx[0] + angular x arm_base_pos + arm_base_rot * vx_arm[i-1]

  // veh base link
  px[0] = veh_base;
  if (J_px_p) (*J_px_p)[0].block<6,3>(0,0) = Hveh_base;
  if (vx) {
    (*vx)[0] = Vector3((*v)[0], (*v)[1], 0.0);
    // (*J_vx_p)[0] is zero
    if (J_vx_v)
      (*J_vx_v)[0].block<2,2>(0,0) = Matrix2::Identity();
  }

  // arm links
  vector<Pose3> armjpx;
  vector<Vector3> armjvx;
  vector<Matrix> Jarm_jpx_jp, Jarm_jvx_jp, Jarm_jvx_jv;

  arm_.updateBasePose(arm_base);
  if (v) {
    const Vector varm = v->tail(arm_.dof());
    arm_.forwardKinematics(p.configuration(), boost::optional<const Vector&>(varm),
        armjpx, vx ? boost::optional<vector<Vector3>&>(armjvx) : boost::none,
        J_px_p ? boost::optional<vector<Matrix>&>(Jarm_jpx_jp) : boost::none,
        J_vx_p ? boost::optional<vector<Matrix>&>(Jarm_jvx_jp) : boost::none,
        J_vx_v ? boost::optional<vector<Matrix>&>(Jarm_jvx_jv) : boost::none);
  } else {
    arm_.forwardKinematics(p.configuration(), boost::none,
        armjpx, vx ? boost::optional<vector<Vector3>&>(armjvx) : boost::none,
        J_px_p ? boost::optional<vector<Matrix>&>(Jarm_jpx_jp) : boost::none);
  }

  for (size_t i = 0; i < arm_.dof(); i++) {
    px[i+1] = armjpx[i];
    if (J_px_p) {
      // see compose's jacobian
      (*J_px_p)[i+1].block<6,3>(0,0) = (armjpx[i].inverse() * arm_base).AdjointMap() * Harm_base;
      (*J_px_p)[i+1].block(0,3,6,arm_.dof()) = Jarm_jpx_jp[i];
    }
    if (vx) {
      //(*vx)[i+1] = Vector3((*v)[0], (*v)[1], 0.0) +  armjvx[i];
    }
  }

}