290 lines
14 KiB
C++
290 lines
14 KiB
C++
|
#ifdef _WIN32
|
||
|
|
#include <windows.h>
|
||
|
|
#include <GL/gl.h>
|
||
|
|
#include <GL/glu.h>
|
||
|
|
#else
|
||
|
|
#ifdef __APPLE__
|
||
|
|
#include <OpenGL/gl.h>
|
||
|
|
#include <OpenGL/glu.h>
|
||
|
|
#else
|
||
|
|
#include <GL/gl.h>
|
||
|
|
#include <GL/glu.h>
|
||
|
|
#endif
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#include "myworld.h"
|
||
|
|
|
||
|
|
using namespace std;
|
||
|
|
|
||
|
|
void glsubBox(double dx, double dy, double dz)
|
||
|
|
{
|
||
|
|
glBegin(GL_QUADS);
|
||
|
|
glNormal3d(1, 0, 0); glVertex3d(dx/2, -dy/2, dz/2); glVertex3d(dx/2, -dy/2, -dz/2); glVertex3d(dx/2, dy/2, -dz/2); glVertex3d(dx/2, dy/2, dz/2);
|
||
|
|
glEnd();
|
||
|
|
glBegin(GL_QUADS);
|
||
|
|
glNormal3d(0, 1, 0); glVertex3d(dx/2, dy/2, dz/2); glVertex3d(dx/2, dy/2, -dz/2); glVertex3d(-dx/2, dy/2, -dz/2); glVertex3d(-dx/2, dy/2, dz/2);
|
||
|
|
glEnd();
|
||
|
|
glBegin(GL_QUADS);
|
||
|
|
glNormal3d(-1, 0, 0); glVertex3d(-dx/2, dy/2, -dz/2); glVertex3d(-dx/2, -dy/2, -dz/2); glVertex3d(-dx/2, -dy/2, dz/2); glVertex3d(-dx/2, dy/2, dz/2);
|
||
|
|
glEnd();
|
||
|
|
glBegin(GL_QUADS);
|
||
|
|
glNormal3d(0, -1, 0); glVertex3d(-dx/2, -dy/2, dz/2); glVertex3d(-dx/2, -dy/2, -dz/2); glVertex3d(dx/2, -dy/2, -dz/2); glVertex3d(dx/2, -dy/2, dz/2);
|
||
|
|
glEnd();
|
||
|
|
glBegin(GL_QUADS);
|
||
|
|
glNormal3d(0, 0, 1); glVertex3d(dx/2, -dy/2, dz/2); glVertex3d(dx/2, dy/2, dz/2); glVertex3d(-dx/2, dy/2, dz/2); glVertex3d(-dx/2, -dy/2, dz/2);
|
||
|
|
glEnd();
|
||
|
|
glBegin(GL_QUADS);
|
||
|
|
glNormal3d(0, 0, -1); glVertex3d(dx/2, -dy/2, -dz/2); glVertex3d(-dx/2, -dy/2, -dz/2); glVertex3d(-dx/2, dy/2, -dz/2); glVertex3d(dx/2, dy/2, -dz/2);
|
||
|
|
glEnd();
|
||
|
|
}
|
||
|
|
|
||
|
|
//=============================================================
|
||
|
|
// RigidBody
|
||
|
|
//=============================================================
|
||
|
|
void RigidBody::render()
|
||
|
|
{
|
||
|
|
glColor3d(_color[0],_color[1],_color[2]);
|
||
|
|
glPushMatrix();
|
||
|
|
glMultMatrixd(getPoseGlobal().GetArray()); // move to {body}
|
||
|
|
glMultMatrixd(_T0.GetArray()); // move to box's reference frame
|
||
|
|
glsubBox(_Lx, _Ly, _Lz);
|
||
|
|
glPopMatrix();
|
||
|
|
}
|
||
|
|
|
||
|
|
//=============================================================
|
||
|
|
// MyWorld
|
||
|
|
//=============================================================
|
||
|
|
bool MyWorld::create()
|
||
|
|
{
|
||
|
|
//----------------------------------------
|
||
|
|
// link
|
||
|
|
// -----------------------------
|
||
|
|
// | z |
|
||
|
|
// | | | (A local coordinate frame {link} is attached to each link.)
|
||
|
|
// | ---> x |
|
||
|
|
// | |
|
||
|
|
// -----------------------------
|
||
|
|
//
|
||
|
|
// Note: There is no restriction on where the local coordinate frame must be located.
|
||
|
|
// In this illustrative example, {link} is attached to the center of mass, i.e., {link} = {com}.
|
||
|
|
//
|
||
|
|
//----------------------------------------
|
||
|
|
// two-link model
|
||
|
|
//
|
||
|
|
// Z
|
||
|
|
// |
|
||
|
|
// O=-->X====O========== (X,Y,Z) represents the world coordinate frame {global} ({global} is the same as the coordinate frame of ground.)
|
||
|
|
// j1 link1 j2 link2
|
||
|
|
//
|
||
|
|
//----------------------------------------
|
||
|
|
|
||
|
|
double Lx = 0.3, Ly = 0.05, Lz = 0.05; // link dimension
|
||
|
|
double M = 1; // link mass
|
||
|
|
double Ixx = 1./12.*M*(Ly*Ly+Lz*Lz), Iyy = 1./12.*M*(Lx*Lx+Lz*Lz), Izz = 1./12.*M*(Lx*Lx+Ly*Ly), Ixy = 0, Ixz = 0, Iyz = 0; // moments of inertia with respect to the mass center
|
||
|
|
SE3 T_link_com = SE3(); // transformation from {link} to {com} (In this example, T_link_com = identity because {link} = {com}.)
|
||
|
|
|
||
|
|
// set mass and moments of inertia of each link
|
||
|
|
link1.setMass(M, Ixx, Iyy, Izz, Ixy, Ixz, Iyz, T_link_com); // Internally, moments of inertia with respect to {link} are calculated using the moments of inertia with respect to {com} and T_link_com.
|
||
|
|
link2.setMass(M, Ixx, Iyy, Izz, Ixy, Ixz, Iyz, T_link_com);
|
||
|
|
|
||
|
|
// set box rendering (optional)
|
||
|
|
link1.setBoxRendering(Lx, Ly, Lz, SE3(), Vec3(0,0,1));
|
||
|
|
link2.setBoxRendering(Lx, Ly, Lz, SE3(), Vec3(1,0,0));
|
||
|
|
|
||
|
|
// connect the links with joints
|
||
|
|
// Let's think about connecting two links (linkA and linkB) with a joint.
|
||
|
|
// A joint has two reference coordinate frames {joint left} and {joint right} which are attached to linkA and linkB respectively,
|
||
|
|
// and {joint left} and {joint right} are aligned with each other when the joint angle or displacement is zero.
|
||
|
|
// usage:
|
||
|
|
// joint.connectBodies(linkA, linkB) connects linkA and linkB with joint.
|
||
|
|
// joint.setPosition(pA, pB) sets the positions of {joint left} and {joint right} with respect to {linkA} and {linkB} respectively.
|
||
|
|
// joint.setOrientation(Ra, Rb) sets the orientations of {joint left} and {joint right} with respect to {linkA} and {linkB} respectively.
|
||
|
|
joint1.connectBodies(&ground, &link1); joint1.setPosition(Vec3(0,0,0), Vec3(-0.5*Lx,0,0)); // (0,0,0) = position of joint1 in {global}, (-0.5*Lx,0,0) = position of join1 in {link1}
|
||
|
|
joint2.connectBodies(&link1, &link2); joint2.setPosition(Vec3(0.5*Lx,0,0), Vec3(-0.5*Lx,0,0));
|
||
|
|
|
||
|
|
// set rotating axes of the revolute joints
|
||
|
|
joint1.setAxis(0,1,0); // rotation axis with respect to {joint left}
|
||
|
|
joint2.setAxis(0,1,0);
|
||
|
|
|
||
|
|
// build the two-link system
|
||
|
|
system.buildSystem(&ground); // GEAR scans the link-joint structure automatically from ground
|
||
|
|
|
||
|
|
// set gravity
|
||
|
|
system.setGravity(Vec3(0,0,-9.81));
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
void MyWorld::render()
|
||
|
|
{
|
||
|
|
for (std::list<GBody*>::iterator iter_pbody = system.pBodies.begin(); iter_pbody != system.pBodies.end(); iter_pbody++) {
|
||
|
|
(*iter_pbody)->render();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void MyWorld::testfunc()
|
||
|
|
{
|
||
|
|
RMatrix q = Rand(2,1), dq = 0.1*Rand(2,1), ddq, tau;
|
||
|
|
|
||
|
|
// getting basic information
|
||
|
|
cout << "dof = " << system.getNumCoordinates() << endl;
|
||
|
|
cout << "mass = " << system.getMass() << endl;
|
||
|
|
|
||
|
|
// testing kinematics
|
||
|
|
{
|
||
|
|
system.set_q(q); // set joint position
|
||
|
|
system.set_dq(dq); // set joint velocity
|
||
|
|
system.set_ddq(RMatrix("1 2")); // set joint acceleration (unit: rad)
|
||
|
|
system.updateKinematics(); // update position, velocity, and acceleration of the system (links and joints)
|
||
|
|
cout << "p1cg = " << link1.getPositionCOMGlobal() << endl; // position of link1's center of mass w.r.t. {global}
|
||
|
|
cout << "p1g = " << link1.getPositionGlobal() << endl; // position of link1's local coordinate frame origin w.r.t. {global}
|
||
|
|
cout << "v2cg = " << link2.getVelocityCOMGlobal() << endl; // velocity of link2's center of mass w.r.t. {global}
|
||
|
|
cout << "p2g = " << link2.getPositionGlobal() << endl; // position of link2's local coordinate frame origin w.r.t. {global}
|
||
|
|
se3 V2g = link2.getVelocityGlobal(); // generalized velocity (angular velocity, linear velocity) of link2 w.r.t. {global}
|
||
|
|
se3 V2 = link2.V; // generalized velocity of link2 w.r.t. {link2} ({link2} = local body coordinate frame of link2)
|
||
|
|
system.updateSystemJacobianOfAllBodies(); // update Jacobian matrices of all links using current system position and velocity
|
||
|
|
RMatrix J2 = link2.Jacobian; // Jacobian of link2 mapping the system velocity dq to link2's generalized velocity V2 (w.r.t. {link2})
|
||
|
|
cout << "Norm(V2-J2*dq) = " << FNorm(RMatrix(6,1,V2.GetArray()) - J2 * system.get_dq()) << endl; // this must be zeros (V2 = J2*dq)
|
||
|
|
SO3 R2 = link2.getOrientationGlobal(); // orientation of {link2} w.r.t. {global}
|
||
|
|
RMatrix R2_R2 = Zeros(6,6); R2_R2.Push(0,0,RMatrix(3,3,R2.GetArray())); R2_R2.Push(3,3,RMatrix(3,3,R2.GetArray())); // R2_R2 = [R2, 0; 0 R2]
|
||
|
|
RMatrix J2g = R2_R2 * J2; // Jacobian of link2 mapping the system velocity dq to link2's generalized velocity V2g (w.r.t. {global})
|
||
|
|
cout << "Norm(V2g-J2g*dq) = " << FNorm(RMatrix(6,1,V2g.GetArray()) - J2g * system.get_dq()) << endl; // this must be zeros (V2g = J2g*dq)
|
||
|
|
}
|
||
|
|
|
||
|
|
// testing hybrid dynamics: (q,dq,ddq_a,tau_b) --> (tau_a,ddq_b) where 'a' = prescribed joints, 'b' = unprescribed joints
|
||
|
|
// (inverse and forward dynamics are the two extreme cases of the hybrid dynamics)
|
||
|
|
{
|
||
|
|
joint1.setPrescribed(false); // joint 1 is 'unprescribed' (i.e., input = torque)
|
||
|
|
joint2.setPrescribed(true); // joint 2 is 'prescribed' (i.e., input = acceleration)
|
||
|
|
system.set_q(q); // set joint position
|
||
|
|
system.set_dq(dq); // set joint velocity
|
||
|
|
system.set_ddq(RMatrix("1 2")); // set joint acceleration (only acceleration for prescribed joints will be considered in calcDynamics())
|
||
|
|
system.set_tau(RMatrix("0.1 0.2")); // set joint torque (only torque for unprescribed joints will be will be considered in calcDynamics())
|
||
|
|
system.calcDynamics(); // compute torque for prescribed joints and acceleration for unprescribed joints
|
||
|
|
cout << "(ddq1,tau2) = (" << joint1.coordinate.ddq << ", " << joint2.coordinate.tau << ")" << endl; // print joint1's acceleration and joint2's torque
|
||
|
|
}
|
||
|
|
|
||
|
|
// testing inverse dynamics: (q,dq,ddq) --> tau
|
||
|
|
{
|
||
|
|
system.set_q(q); // set joint position
|
||
|
|
system.set_dq(dq); // set joint velocity
|
||
|
|
system.set_ddq(RMatrix("1 2")); // set joint acceleration
|
||
|
|
system.calcInverseDynamics(); // compute joint torque using inverse dynamics (All joints are temporarily regarded as "prescribed" joints.)
|
||
|
|
cout << "tau = " << system.get_tau() << endl; // print joint torque
|
||
|
|
}
|
||
|
|
|
||
|
|
// testing forward dynamics: (q,dq,tau) --> ddq
|
||
|
|
{
|
||
|
|
system.set_q(q); // set joint position
|
||
|
|
system.set_dq(dq); // set joint velocity
|
||
|
|
system.set_tau(RMatrix("0.1 0.2")); // set joint torque
|
||
|
|
system.calcForwardDynamics(); // compute joint acceleration using forward dynamics (All joints are temporarily regarded as "unprescribed" joints.)
|
||
|
|
cout << "ddq = " << system.get_ddq() << endl; // print joint acceleration
|
||
|
|
}
|
||
|
|
|
||
|
|
// testing derivatives of dynamics
|
||
|
|
{
|
||
|
|
joint1.setPrescribed(false); // joint 1 is 'unprescribed'
|
||
|
|
joint2.setPrescribed(false); // joint 2 is 'unprescribed'
|
||
|
|
system.set_q(q); // set joint position
|
||
|
|
system.set_dq(dq); // set joint velocity
|
||
|
|
system.set_tau(RMatrix("0.1 0.2")); // set joint torque
|
||
|
|
system.calcDynamics(); // you must call this first before calling diffDynamics()
|
||
|
|
system.setDeriv_Dq(&joint1.coordinate); // set q1 as the differentiating variable
|
||
|
|
system.diffDynamics(); // compute D(ddq)/D(q1)
|
||
|
|
cout << "D(ddq)/D(q1) = " << system.get_DddqDp() << endl;
|
||
|
|
system.setDeriv_Ddq(&joint1.coordinate); // set dq1 as the differentiating variable
|
||
|
|
system.diffDynamics(); // compute D(ddq)/D(dq1)
|
||
|
|
cout << "D(ddq)/D(dq1) = " << system.get_DddqDp() << endl;
|
||
|
|
system.setDeriv_Dtau(&joint1.coordinate); // set tau1 as the differentiating variable
|
||
|
|
system.diffDynamics(); // compute D(ddq)/D(tau1)
|
||
|
|
cout << "D(ddq)/D(tau1) = " << system.get_DddqDp() << endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
cout << "comparing analytical and numerical derivatives..." << endl;
|
||
|
|
// compare analytical and numerical derivatives: D(ddq)/D(q)
|
||
|
|
{
|
||
|
|
joint1.setPrescribed(false); // joint 1 is 'unprescribed'
|
||
|
|
joint2.setPrescribed(false); // joint 2 is 'unprescribed'
|
||
|
|
system.set_q(q); // set joint position
|
||
|
|
system.set_dq(dq); // set joint velocity
|
||
|
|
system.set_tau(RMatrix("0.1 0.2")); // set joint torque
|
||
|
|
system.calcDynamics(); // you must call this first before calling diffDynamics()
|
||
|
|
vector<GCoordinate*> pcoords = system.getCoordinates();
|
||
|
|
RMatrix DddqDq(2,2); // analytical derivative DddqDq = [D(ddq)/D(q1), D(ddq)/D(q2)]
|
||
|
|
for (int i=0; i<pcoords.size(); i++) {
|
||
|
|
system.setDeriv_Dq(pcoords[i]);
|
||
|
|
system.diffDynamics();
|
||
|
|
DddqDq.Push(0,i,system.get_DddqDp());
|
||
|
|
}
|
||
|
|
RMatrix NDddqDq(2,2); // numerical derivative NDddqDq = [D(ddq)/D(q1), D(ddq)/D(q2)]
|
||
|
|
RMatrix ddq0 = system.get_ddq();
|
||
|
|
double h = 1E-6;
|
||
|
|
for (int i=0; i<pcoords.size(); i++) {
|
||
|
|
pcoords[i]->q += h;
|
||
|
|
system.calcDynamics();
|
||
|
|
NDddqDq.Push(0,i,(1./h)*(system.get_ddq()-ddq0));
|
||
|
|
pcoords[i]->q -= h;
|
||
|
|
}
|
||
|
|
//cout << "DddqDq = " << DddqDq << "NDddqDq = " << NDddqDq << endl;
|
||
|
|
cout << "Norm(DddqDq-NDddqDq) = " << FNorm(DddqDq-NDddqDq) << endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
// compare analytical and numerical derivatives: D(ddq)/D(dq)
|
||
|
|
{
|
||
|
|
joint1.setPrescribed(false); // joint 1 is 'unprescribed'
|
||
|
|
joint2.setPrescribed(false); // joint 2 is 'unprescribed'
|
||
|
|
system.set_q(q); // set joint position
|
||
|
|
system.set_dq(dq); // set joint velocity
|
||
|
|
system.set_tau(RMatrix("0.1 0.2")); // set joint torque
|
||
|
|
system.calcDynamics(); // you must call this first before calling diffDynamics()
|
||
|
|
vector<GCoordinate*> pcoords = system.getCoordinates();
|
||
|
|
RMatrix DddqDdq(2,2); // analytical derivative DddqDdq = [D(ddq)/D(dq1), D(ddq)/D(dq2)]
|
||
|
|
for (int i=0; i<pcoords.size(); i++) {
|
||
|
|
system.setDeriv_Ddq(pcoords[i]);
|
||
|
|
system.diffDynamics();
|
||
|
|
DddqDdq.Push(0,i,system.get_DddqDp());
|
||
|
|
}
|
||
|
|
RMatrix NDddqDdq(2,2); // numerical derivative NDddqDdq = [D(ddq)/D(dq1), D(ddq)/D(dq2)]
|
||
|
|
RMatrix ddq0 = system.get_ddq();
|
||
|
|
double h = 1E-6;
|
||
|
|
for (int i=0; i<pcoords.size(); i++) {
|
||
|
|
pcoords[i]->dq += h;
|
||
|
|
system.calcDynamics();
|
||
|
|
NDddqDdq.Push(0,i,(1./h)*(system.get_ddq()-ddq0));
|
||
|
|
pcoords[i]->dq -= h;
|
||
|
|
}
|
||
|
|
//cout << "DddqDdq = " << DddqDdq << "NDddqDdq = " << NDddqDdq << endl;
|
||
|
|
cout << "Norm(DddqDdq-NDddqDdq) = " << FNorm(DddqDdq-NDddqDdq) << endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
// compare analytical and numerical derivatives: D(ddq)/D(tau)
|
||
|
|
{
|
||
|
|
joint1.setPrescribed(false); // joint 1 is 'unprescribed'
|
||
|
|
joint2.setPrescribed(false); // joint 2 is 'unprescribed'
|
||
|
|
system.set_q(q); // set joint position
|
||
|
|
system.set_dq(dq); // set joint velocity
|
||
|
|
system.set_tau(RMatrix("0.1 0.2")); // set joint torque
|
||
|
|
system.calcDynamics(); // you must call this first before calling diffDynamics()
|
||
|
|
vector<GCoordinate*> pcoords = system.getCoordinates();
|
||
|
|
RMatrix DddqDtau(2,2); // analytical derivative DddqDtau = [D(ddq)/D(tau1), D(ddq)/D(tau2)]
|
||
|
|
for (int i=0; i<pcoords.size(); i++) {
|
||
|
|
system.setDeriv_Dtau(pcoords[i]);
|
||
|
|
system.diffDynamics();
|
||
|
|
DddqDtau.Push(0,i,system.get_DddqDp());
|
||
|
|
}
|
||
|
|
RMatrix NDddqDtau(2,2); // numerical derivative NDddqDtau = [D(ddq)/D(tau1), D(ddq)/D(tau2)]
|
||
|
|
RMatrix ddq0 = system.get_ddq();
|
||
|
|
double h = 1E-6;
|
||
|
|
for (int i=0; i<pcoords.size(); i++) {
|
||
|
|
pcoords[i]->tau += h;
|
||
|
|
system.calcDynamics();
|
||
|
|
NDddqDtau.Push(0,i,(1./h)*(system.get_ddq()-ddq0));
|
||
|
|
pcoords[i]->tau -= h;
|
||
|
|
}
|
||
|
|
//cout << "DddqDtau = " << DddqDtau << "NDddqDtau = " << NDddqDtau << endl;
|
||
|
|
cout << "Norm(DddqDtau-NDddqDtau) = " << FNorm(DddqDtau-NDddqDtau) << endl;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|