12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix Vector_() to Vec() in gtsam_unstable/dynamics

release/4.3a0
Jing Dong 2013-10-22 04:22:50 +00:00
parent 181881a8d7
commit 6587f9781c
6 changed files with 25 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -126,7 +126,7 @@ PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate,
const Velocity3& v1 = v();
// Update vehicle heading
Rot3 r2 = r1.retract(Vector_(3, 0.0, 0.0, heading_rate * dt));
Rot3 r2 = r1.retract((Vec(3) << 0.0, 0.0, heading_rate * dt));
const double yaw2 = r2.ypr()(0);
// Update vehicle position
@ -150,7 +150,7 @@ PoseRTV PoseRTV::flyingDynamics(
const Velocity3& v1 = v();
// Update vehicle heading (and normalise yaw)
Vector rot_rates = Vector_(3, 0.0, pitch_rate, heading_rate);
Vector rot_rates = (Vec(3) << 0.0, pitch_rate, heading_rate);
Rot3 r2 = r1.retract(rot_rates*dt);
// Work out dynamics on platform
@ -165,7 +165,7 @@ PoseRTV PoseRTV::flyingDynamics(
Point3 forward(forward_accel, 0.0, 0.0);
Vector Acc_n =
yaw_correction_bn.rotate(forward).vector() // applies locally forward force in the global frame
- drag * Vector_(3, v1.x(), v1.y(), 0.0) // drag term dependent on v1
- drag * (Vec(3) << v1.x(), v1.y(), 0.0) // drag term dependent on v1
+ delta(3, 2, loss_lift - lift_control); // falling due to lift lost from pitch
// Update Vehicle Position and Velocity

4
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -126,7 +126,7 @@ public:
Matrix D_gravityBody_gk;
Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk);
Vector f_ext = Vector_(6, 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
Vector f_ext = (Vec(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;
@ -154,7 +154,7 @@ public:
Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1;
Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_));
Vector f_ext = Vector_(6, 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
Vector f_ext = (Vec(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z());
Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext;

12
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) {
// create a simple chain of poses to generate IMU measurements
const double dt = 1.0;
PoseRTV pose1,
pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Vector_(3, 2.0, 2.0, 0.0)),
pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Vector_(3, 0.0, 0.0, 0.0)),
pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Vector_(3, 2.0, 2.0, 0.0));
pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), (Vector)(Vec(3) << 2.0, 2.0, 0.0)),
pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), (Vector)(Vec(3) << 0.0, 0.0, 0.0)),
pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), (Vector)(Vec(3) << 2.0, 2.0, 0.0));
// create measurements
SharedDiagonal model = noiseModel::Unit::Create(6);
@ -103,9 +103,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
// create a simple chain of poses to generate IMU measurements
const double dt = 1.0;
PoseRTV pose1,
pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Vector_(3, 1.0, 0.0, 0.0)),
pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Vector_(3, 1.0, 0.0, 0.0)),
pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Vector_(3, 1.0, 0.0, 0.0));
pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vec(3) << 1.0, 0.0, 0.0)),
pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vec(3) << 1.0, 0.0, 0.0)),
pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vec(3) << 1.0, 0.0, 0.0));
// create measurements
SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0);

10
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -47,7 +47,7 @@ TEST( testPoseRTV, constructors ) {
EXPECT(assert_equal(Velocity3(), state4.v(), tol));
EXPECT(assert_equal(Pose3(rot, pt), state4.pose(), tol));
Vector vec_init = Vector_(9, 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6);
Vector vec_init = (Vec(9) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6);
PoseRTV state5(vec_init);
EXPECT(assert_equal(pt, state5.t(), tol));
EXPECT(assert_equal(rot, state5.R(), tol));
@ -83,7 +83,7 @@ TEST( testPoseRTV, Lie ) {
EXPECT(assert_equal(state1, state1.retract(zero(9)), tol));
EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol));
Vector delta = Vector_(9, 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3);
Vector delta = (Vec(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3);
Rot3 rot2 = rot.retract(repeat(3, 0.1));
Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4);
Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3);
@ -99,7 +99,7 @@ TEST( testPoseRTV, dynamics_identities ) {
PoseRTV x0, x1, x2, x3, x4;
const double dt = 0.1;
Vector accel = Vector_(3, 0.2, 0.0, 0.0), gyro = Vector_(3, 0.0, 0.0, 0.2);
Vector accel = (Vec(3) << 0.2, 0.0, 0.0), gyro = (Vec(3) << 0.0, 0.0, 0.2);
Vector imu01 = zero(6), imu12 = zero(6), imu23 = zero(6), imu34 = zero(6);
x1 = x0.generalDynamics(accel, gyro, dt);
@ -181,14 +181,14 @@ TEST( testPoseRTV, transformed_from_2 ) {
TEST(testPoseRTV, RRTMbn) {
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(zero(3)), tol));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()), tol));
EXPECT(assert_equal(PoseRTV::RRTMbn(Vector_(3, 0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol));
EXPECT(assert_equal(PoseRTV::RRTMbn((Vec(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol));
}
/* ************************************************************************* */
TEST(testPoseRTV, RRTMnb) {
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(zero(3)), tol));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()), tol));
EXPECT(assert_equal(PoseRTV::RRTMnb(Vector_(3, 0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol));
EXPECT(assert_equal(PoseRTV::RRTMnb((Vec(3) << 0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol));
}
/* ************************************************************************* */

14
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -18,19 +18,19 @@ const double deg2rad = M_PI/180.0;
//Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0));
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0));
//LieVector v1(Vector_(6, 0.1, 0.05, 0.02, 10.0, 20.0, 30.0));
LieVector V1_w(Vector_(6, 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0));
//LieVector v1((Vec(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0));
LieVector V1_w((Vec(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0));
LieVector V1_g1 = g1.inverse().Adjoint(V1_w);
Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP));
//LieVector v2 = Pose3::Logmap(g1.between(g2));
double mass = 100.0;
Vector gamma2 = Vector_(2, 0.0, 0.0); // no shape
Vector u2 = Vector_(2, 0.0, 0.0); // no control at time 2
Vector gamma2 = (Vec(2) << 0.0, 0.0); // no shape
Vector u2 = (Vec(2) << 0.0, 0.0); // no control at time 2
double distT = 1.0; // distance from the body-centered x axis to the big top motor
double distR = 5.0; // distance from the body-centered z axis to the small motor
Matrix Mass = diag(Vector_(3, mass, mass, mass));
Matrix Inertia = diag(Vector_(6, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass));
Matrix Mass = diag((Vec(3) << mass, mass, mass));
Matrix Inertia = diag((Vec(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass));
Vector computeFu(const Vector& gamma, const Vector& control) {
double gamma_r = gamma(0), gamma_p = gamma(1);
@ -100,7 +100,7 @@ TEST( Reconstruction, evaluateError) {
/* ************************************************************************* */
// Implement Newton-Euler equation for rigid body dynamics
Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) {
Matrix W = Pose3::adjointMap(Vector_(6, Vb(0), Vb(1), Vb(2), 0., 0., 0.));
Matrix W = Pose3::adjointMap((Vec(6) << Vb(0), Vb(1), Vb(2), 0., 0., 0.));
Vector dV = Inertia.inverse()*(Fb - W*Inertia*Vb);
return dV;
}

4
gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
View File

@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2;
const double dt = 1.0;
PoseRTV origin,
pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Vector_(3, 1.0, 0.0, 0.0)),
pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vec(3) << 1.0, 0.0, 0.0)),
pose1a(Point3(0.5, 0.0, 0.0)),
pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Vector_(3, 1.0, 0.0, 0.0));
pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vec(3) << 1.0, 0.0, 0.0));
/* ************************************************************************* */
TEST( testVelocityConstraint, trapezoidal ) {