inline void ImplicitCapillarity<GI, RP, BC, IP>::initObj(const GI& g, const RP& r, const BC& b)
{
residual_.initObj(g, r, b);
Dune::FieldVector<double, GI::Dimension> grav(0.0);
psolver_.init(g, r, grav, b);
}
void SingleSupportModel::doInverseDynamics(bool normalize, DataSource source)
{
if(m_coeff == 0 && normalize)
return;
double coeff = m_normalizedCoeff;
if(!normalize)
coeff = 1.0;
updateKinematics(source);
tf::Vector3 grav(0, 0, -9.81);
grav = tf::Transform(m_model->robotOrientation()).inverse() * grav;
// Transform grav to base coordinates
if(m_trunkJoint != -1)
{
const Math::SpatialTransform X = X_base[m_trunkJoint];
gravity = X.E.transpose() * Math::Vector3d(grav.x(), grav.y(), grav.z());
}
RigidBodyDynamics::InverseDynamics(*this, m_q, m_qdot, m_qdotdot, m_tau, 0);
for(size_t i = 0; i < m_joints.size(); ++i)
{
if(!m_joints[i])
continue;
m_joints[i]->feedback.modelTorque += coeff * m_tau[i];
}
}
/** Fills a vector with the Q values calculated from the wavelength bin centers from the input workspace and
* the workspace geometry as Q = 4*pi*sin(theta)/lambda
* @param[in] specInd the spectrum to calculate
* @param[in] doGravity if to include gravity in the calculation of Q
* @param[in] offset index number of the first input bin to use
* @param[out] Qs points to a preallocated array that is large enough to contain all the calculated Q values
* @throw NotFoundError if the detector associated with the spectrum is not found in the instrument definition
*/
void Q1D2::convertWavetoQ(const size_t specInd, const bool doGravity, const size_t offset, MantidVec::iterator Qs) const
{
static const double FOUR_PI=4.0*M_PI;
IDetector_const_sptr det = m_dataWS->getDetector(specInd);
// wavelengths (lamda) to be converted to Q
MantidVec::const_iterator waves = m_dataWS->readX(specInd).begin() + offset;
// going from bin boundaries to bin centered x-values the size goes down one
const MantidVec::const_iterator end = m_dataWS->readX(specInd).end() - 1;
if (doGravity)
{
GravitySANSHelper grav(m_dataWS, det);
for( ; waves !=end; ++Qs, ++waves)
{
// the HistogramValidator at the start should ensure that we have one more bin on the input wavelengths
const double lambda = 0.5*(*(waves+1)+(*waves));
// as the fall under gravity is wavelength dependent sin theta is now different for each bin with each detector
const double sinTheta = grav.calcSinTheta(lambda);
// Now we're ready to go to Q
*Qs = FOUR_PI*sinTheta/lambda;
}
}
else
{
// Calculate the Q values for the current spectrum, using Q = 4*pi*sin(theta)/lambda
const double factor = 2.0* FOUR_PI*sin( m_dataWS->detectorTwoTheta(det)/2.0 );
for( ; waves !=end; ++Qs, ++waves)
{
// the HistogramValidator at the start should ensure that we have one more bin on the input wavelengths
*Qs = factor/(*(waves+1)+(*waves));
}
}
}
/**
* Calcule le centre de gravité
**/
ofPoint ARModule::makeGravCenter(vector <ofPoint> pts){
ofPoint grav(0,0);
for(int i=0;i<pts.size();i++){
grav.x+=pts[i].x;
grav.y+=pts[i].y;
}
grav.x/=pts.size();
grav.y/=pts.size();
return grav;
}
void ArenaLocker::Setup(InputManager* Input,GraphicsManager* Graphics,GUIManager* Gui,SoundManager* Sound)
{
_scene = Graphics->createSceneManager(Ogre::ST_INTERIOR,"arenaLocker");
_rootNode = _scene->getRootSceneNode();
_camera = _scene->createCamera("arenaLockerCam");
_camera->setAspectRatio(16.0f / 9.0f);
_camera->setNearClipDistance(0.001f);
_view = Graphics->getRenderWindow()->addViewport(_camera);
_view->setBackgroundColour(Ogre::ColourValue(0,0,0));
std::cout << "Arena Locker - scene manager and camera/viewport created" << std::endl;
_physics.reset(new PhysicsManager());
btVector3 grav(0.0f,-9.8f,0.0f);
_physics->Setup(grav);
std::cout << "Arena Locker - physics setup" << std::endl;
//list of physics-based entities and such
_loadPhysicsEntities("resource\\xml\\lists\\arenalocker_list.xml");
std::cout << "Arena Locker - models loaded" << std::endl;
std::vector<LevelData::Waypoint> waypoints;
LevelData::LevelParser parser;
parser.setFile("resource\\models\\arena_locker\\arenalocker_test.ent");
parser.parseWaypoints(&waypoints);
std::cout << "Arena Locker - parser finished" << std::endl;
Ogre::Entity* levelEnt = static_cast<Ogre::Entity*>(_pairs.begin()->ogreNode->getAttachedObject(0));
_navigationMeshSetup(levelEnt);
_AIManager.reset(new AIManager());
_AIManager->loadNPCs("resource\\xml\\lists\\arenalocker_npc_list.xml",_crowd.get(),_scene,.9f);
std::cout << "NPCs loaded" << std::endl;
_loadSounds("resource\\xml\\arena_locker\\locker_soundlist.xml",Sound);
std::cout << "Sounds loaded" << std::endl;
_pauseMenu.reset(new PauseMenu(State::GAME_LOCKER));
_pauseMenu->Setup(Input,Graphics,Gui,Sound);
std::cout << "Pause menu initialized" << std::endl;
_oldCameraPositionTarget = Ogre::Vector3(-7,1.0f,-8.0f);
LuaManager::getSingleton().callFunction("Arena_InitCameraMovement",2);
_cameraPositionTarget = LuaManager::getVectorFromLua(LuaManager::getSingletonPtr()->getLuaState(),1);
Ogre::Vector3 startingLook = LuaManager::getVectorFromLua(LuaManager::getSingletonPtr()->getLuaState(),2);
_oldCameraLookTarget = startingLook;
_cameraLookTarget = startingLook;
_camera->rotate(_camera->getDirection().getRotationTo(startingLook));
_oldCameraOrientation = _camera->getOrientation();
LuaManager::getSingleton().clearLuaStack();
//TEST
_sphere = Graphics->createSceneNode(_scene,object("resource/xml/test_sphere.xml").get(),_rootNode);
//TEST
}
// 布の形状の更新
void updateCloth(void) {
// ★ 次の手順で質点の位置を決定する
//clothのみがグローバル変数
// 1. 質点に働く力を求める
// 質点のfの定義
for(int y = 0; y < POINT_NUM; y++) {
for(int x = 0; x < POINT_NUM; x++) {
cloth->points[x][y].f.set(0,0,0);
}
}
//バネによる力を考える
for(int i = 0; i < cloth->springs.size(); i++) {
Vector3d nowlength(cloth->springs[i]->p0->p - cloth->springs[i]->p1->p);
double gap = nowlength.length() - cloth->springs[i]->restLength;
double strpower = Ks * gap;
//p1-p0
Vector3d attraction(cloth->springs[i]->p1->p.x - cloth->springs[i]->p0->p.x,
cloth->springs[i]->p1->p.y - cloth->springs[i]->p0->p.y,
cloth->springs[i]->p1->p.z - cloth->springs[i]->p0->p.z);
attraction.normalize();
attraction.scale(strpower);
cloth->springs[i]->p0->f += attraction;
//向きを反転
attraction.scale(-1);
cloth->springs[i]->p1->f += attraction;
}
//重力M*gを加える
//ついでに空気抵抗を考える
for(int y = 0; y < POINT_NUM; y++) {
for(int x = 0; x < POINT_NUM; x++) {
Vector3d grav(Mass*gravity.x,Mass*gravity.y,Mass*gravity.z);
cloth->points[x][y].f += grav;
Vector3d airresister(Dk*cloth->points[x][y].v.x,Dk*cloth->points[x][y].v.y,Dk*cloth->points[x][y].v.z);
cloth->points[x][y].f -= airresister;
}
}
// 2. 質点の加速度を求める
// 3. 質点の速度を更新する
// 4. 質点の位置を更新する
for(int y = 0; y < POINT_NUM; y++){
for(int x = 0; x < POINT_NUM; x++) {
//加速度accelを求めてdTを掛けた
if(!(cloth->points[x][y].bFixed)){
Vector3d accel(cloth->points[x][y].f.x/Mass*dT,cloth->points[x][y].f.y/Mass*dT,cloth->points[x][y].f.z/Mass*dT);
//質点の速度
cloth->points[x][y].v += accel;
//質点の位置
Vector3d xx(cloth->points[x][y].v.x*dT,cloth->points[x][y].v.y*dT,cloth->points[x][y].v.z*dT);
cloth->points[x][y].p += xx;
}
}
}
}
//todo: allow to create solver, broadphase, multiple worlds etc.
static int gCreateDefaultDynamicsWorld(lua_State *L)
{
sLuaDemo->createEmptyDynamicsWorld();
btVector3 grav(0,0,0);
grav[upaxis] = -10;
sLuaDemo->m_dynamicsWorld->setGravity(grav);
sLuaDemo->m_guiHelper->createPhysicsDebugDrawer(sLuaDemo->m_dynamicsWorld);
lua_pushlightuserdata (L, sLuaDemo->m_dynamicsWorld);
return 1;
}
int main () {
double pos[3] = {0, 0, 0};
double mass = 1;
particle part;
pointMass p (mass, pos, &part);
gravity grav (&p, 9.8);
applied app (&part, 5, {1});
jEng jamie ({&p}, {&grav, &app}, 0.1);
for (unsigned i=0; i < 10; i++) {
jamie.update();
}
}
void RobotControlExample::initPhysics()
{
///for this testing we use Z-axis up
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
createEmptyDynamicsWorld();
//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
btVector3 grav(0,0,0);
grav[upAxis] = 0;//-9.8;
this->m_dynamicsWorld->setGravity(grav);
bool allowSharedMemoryInitialization = true;
m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper);
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
bool isTrigger = false;
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
createButton("Set Physics Params",CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,isTrigger);
createButton("Init Pose",CMD_INIT_POSE,isTrigger);
} else
{
/*
m_userCommandRequests.push_back(CMD_LOAD_URDF);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SHUTDOWN);
*/
}
if (!m_physicsClient.connect())
{
b3Warning("Cannot eonnect to physics client");
}
}
inline ImplicitCapillarity<GI, RP, BC, IP>::ImplicitCapillarity(const GI& g, const RP& r, const BC& b)
: residual_(g, r, b),
method_viscous_(true),
method_gravity_(true),
check_sat_(true),
clamp_sat_(false),
residual_tolerance_(1e-8),
linsolver_verbosity_(1),
linsolver_type_(1),
update_relaxation_(1.0)
{
Dune::FieldVector<double, GI::Dimension> grav(0.0);
psolver_.init(g, r, grav, b);
}
static void GravityChanged_Callback(IConVar *var, const char *pOldString, float)
{
ConVarRef grav(var);
UpdatePhysicsGravity(grav.GetFloat());
if (gpGlobals->mapname != NULL_STRING)
{
IGameEvent *event = gameeventmanager->CreateEvent("gravity_change");
if (event)
{
event->SetFloat("newgravity", grav.GetFloat());
gameeventmanager->FireEvent(event);
}
}
}
void PhysicsServerExample::initPhysics()
{
///for this testing we use Z-axis up
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
createEmptyDynamicsWorld();
//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
btVector3 grav(0,0,0);
grav[upAxis] = 0;//-9.8;
this->m_dynamicsWorld->setGravity(grav);
m_isConnected = m_physicsServer.connectSharedMemory( m_dynamicsWorld,m_guiHelper);
}
void ParticleParameters::ApplyChanges()
{
math::vec2f pos(ui->PosXSpin->value(), ui->PosYSpin->value());
math::vec2f grav(ui->GravXSpin->value(), ui->GravYSpin->value());
double frequency = ui->FrequencySpin->value();
int numParticle = ui->NumPartSpin->value();
if (numParticle == -1 && frequency == 0) numParticle = 1;
m_parameters.setAngle(ui->ApertureSpin->value() * DEG2RAD);
m_parameters.setDirection(ui->DirectionSpin->value() * DEG2RAD);
m_parameters.setPosition(pos);
m_parameters.setGravity(grav);
m_parameters.setFrequency(frequency);
m_parameters.setParticleColor(toRGBA(StartColor), toRGBA(EndColor));
m_parameters.setParticleLive(ui->MinLiveSpin->value(), ui->MaxLiveSpin->value());
m_parameters.setParticleNumber(numParticle);
m_parameters.setParticleSpeed(ui->MinSpeedSpin->value(), ui->MaxSpeedSpin->value());
m_parameters.setParticleSize(ui->StartSizeSpin->value(), ui->EndSizeSpin->value());
m_parameters.setParticleAccumulativeColor(ui->AccumColorCheckbox->isChecked());
m_parameters.setParticleMaterial(ui->MaterialLineEdit->text().toStdString());
}
/* ------------------------------------- main program-----------------------------------------*/
void main()
{
back(); /* draw the background (platforms and ladders) */
data_arary(); /* create the collision detection array */
updwn=6; /* first up and down tile number */
current=0; /* first left right tile number */
x=15; /* x cordinate to start at */
y=16; /* y cordinate to start at */
SPRITES_8x16; /* sets sprites to, yep you guessed it, 8x16 mode */
set_sprite_data(0, 16, bloke); /* defines the main sprite data */
set_sprite_tile(0, current);
move_sprite(current, x, y); /* puts the first sprite on screen */
SHOW_SPRITES;
while(!0) /* infinate game loop */
{
grav(); /* check we are stood on something and fall if we are'nt */
player(); /* player movement and collision detection routines */
/* returned values have no meaning here, just for tests */
} /* end of infinate loop */
} /* end main */
void ad_grav_wall::update_attrs(){
// gravity
if( 1 ){
btCollisionObjectArray& objs = world.world->getCollisionObjectArray();
for( int i=0; i<objs.size(); i++ ){
btTransform &trans = objs[i]->getWorldTransform();
btVector3 &bpos = trans.getOrigin();
ofVec3f pos(bpos.x(), bpos.y(), bpos.z());
gvWall * gl = static_cast<gvWall*>( objs[i]->getUserPointer() );
if( gl == NULL )
continue;
ofVec3f grav_dir = ad_geom_util::vec_pl(pos, gl->p1, gl->p2 );
grav_dir.normalize();
btVector3 grav(grav_dir.x, grav_dir.y, grav_dir.z );
btRigidBody* body = btRigidBody::upcast(objs[i]);
body->applyCentralForce( grav * impulse * 400 );
}
}
}
//test intersection using a rudimentary physics simulation
//this drops a particle onto the terrain and it bounces around a bit
int main()
{
Mercator::Terrain terrain;
terrain.setBasePoint(0, 0, 2.8);
terrain.setBasePoint(1, 0, 7.1);
terrain.setBasePoint(0, 1, 0.2);
terrain.setBasePoint(1, 1, 14.7);
Mercator::Segment * segment = terrain.getSegment(0, 0);
if (segment == 0) {
std::cerr << "Segment not created by addition of required basepoints"
<< std::endl << std::flush;
return 1;
}
segment->populate();
WFMath::Point<3> pos(30.0,30.0,100.0); //starting position
WFMath::Vector<3> vel(0.0,1.0,0.0); //starting velocity
WFMath::Vector<3> grav(0.0,0.0,-9.8); //gravity
WFMath::Point<3> intersection;
WFMath::Vector<3> intnormal;
float timestep = 0.1;
float e = 0.2; //elasticity of collision
float totalT = 20.0; //time limit
float par = 0.0;
float t = timestep;
while (totalT > timestep) {
vel += t * grav;
if (Mercator::Intersect(terrain, pos, vel * t, intersection, intnormal, par)) {
//set pos to collision time,
//less a small amout to keep objects apart
pos = intersection - (vel * .01 * t);
WFMath::Vector<3> impulse = intnormal * (Dot(vel, intnormal) * -2);
std::cerr << "HIT" << std::endl;
vel = (vel + impulse) * e; //not sure of the impulse equation, but this will do
if (vel.sqrMag() < 0.01) {
//stop if velocities are small
std::cerr << "friction stop" << std::endl;
break;
}
totalT -= par*t;
t = (1.0-par)*t;
}
else {
pos += vel*t;
totalT -= t;
t = timestep;
}
std::cerr << "timeLeft:" << totalT << " end pos" << pos << " vel" << vel << std::endl;
}
return 0;
}
int main(int argc, char** argv) {
const double PI(3.141592653589793);
if (argc != 4) {
std::cout << "usage:" << std::endl;
std::cout << "package_scene path_scene output" << std::endl;
return 0;
}
ros::init(argc, argv, "dart_test");
ros::NodeHandle nh_;
std::string package_name( argv[1] );
std::string scene_urdf( argv[2] );
ros::Rate loop_rate(400);
ros::Publisher joint_state_pub_;
joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>("/joint_states", 10);
tf::TransformBroadcaster br;
MarkerPublisher markers_pub(nh_);
std::string package_path_barrett = ros::package::getPath("barrett_hand_defs");
std::string package_path = ros::package::getPath(package_name);
// Load the Skeleton from a file
dart::utils::DartLoader loader;
loader.addPackageDirectory("barrett_hand_defs", package_path_barrett);
loader.addPackageDirectory("barrett_hand_sim_dart", package_path);
boost::shared_ptr<GraspSpecification > gspec = GraspSpecification::readFromUrdf(package_path + scene_urdf);
dart::dynamics::SkeletonPtr scene( loader.parseSkeleton(package_path + scene_urdf) );
scene->enableSelfCollision(true);
dart::dynamics::SkeletonPtr bh( loader.parseSkeleton(package_path_barrett + "/robots/barrett_hand.urdf") );
Eigen::Isometry3d tf;
tf = scene->getBodyNode("gripper_mount_link")->getRelativeTransform();
bh->getJoint(0)->setTransformFromParentBodyNode(tf);
dart::simulation::World* world = new dart::simulation::World();
world->addSkeleton(scene);
world->addSkeleton(bh);
Eigen::Vector3d grav(0,0,-1);
world->setGravity(grav);
GripperController gc;
double Kc = 400.0;
double KcDivTi = Kc / 1.0;
gc.addJoint("right_HandFingerOneKnuckleOneJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, true, false);
gc.addJoint("right_HandFingerOneKnuckleTwoJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, false, true);
gc.addJoint("right_HandFingerTwoKnuckleTwoJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, false, true);
gc.addJoint("right_HandFingerThreeKnuckleTwoJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, false, true);
gc.addJointMimic("right_HandFingerTwoKnuckleOneJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, true, "right_HandFingerOneKnuckleOneJoint", 1.0, 0.0);
gc.addJointMimic("right_HandFingerOneKnuckleThreeJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, false, "right_HandFingerOneKnuckleTwoJoint", 0.333333, 0.0);
gc.addJointMimic("right_HandFingerTwoKnuckleThreeJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, false, "right_HandFingerTwoKnuckleTwoJoint", 0.333333, 0.0);
gc.addJointMimic("right_HandFingerThreeKnuckleThreeJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, false, "right_HandFingerThreeKnuckleTwoJoint", 0.333333, 0.0);
gc.setGoalPosition("right_HandFingerOneKnuckleOneJoint", gspec->getGoalPosition("right_HandFingerOneKnuckleOneJoint"));
gc.setGoalPosition("right_HandFingerOneKnuckleTwoJoint", gspec->getGoalPosition("right_HandFingerOneKnuckleTwoJoint"));
gc.setGoalPosition("right_HandFingerTwoKnuckleTwoJoint", gspec->getGoalPosition("right_HandFingerTwoKnuckleTwoJoint"));
gc.setGoalPosition("right_HandFingerThreeKnuckleTwoJoint", gspec->getGoalPosition("right_HandFingerThreeKnuckleTwoJoint"));
std::map<std::string, double> joint_q_map;
joint_q_map["right_HandFingerOneKnuckleOneJoint"] = gspec->getInitPosition("right_HandFingerOneKnuckleOneJoint");
joint_q_map["right_HandFingerTwoKnuckleOneJoint"] = gspec->getInitPosition("right_HandFingerOneKnuckleOneJoint");
joint_q_map["right_HandFingerOneKnuckleTwoJoint"] = gspec->getInitPosition("right_HandFingerOneKnuckleTwoJoint");
joint_q_map["right_HandFingerOneKnuckleThreeJoint"] = 0.333333 * gspec->getInitPosition("right_HandFingerOneKnuckleTwoJoint");
joint_q_map["right_HandFingerTwoKnuckleTwoJoint"] = gspec->getInitPosition("right_HandFingerTwoKnuckleTwoJoint");
joint_q_map["right_HandFingerTwoKnuckleThreeJoint"] = 0.333333 * gspec->getInitPosition("right_HandFingerTwoKnuckleTwoJoint");
joint_q_map["right_HandFingerThreeKnuckleTwoJoint"] = gspec->getInitPosition("right_HandFingerThreeKnuckleTwoJoint");
joint_q_map["right_HandFingerThreeKnuckleThreeJoint"] = 0.333333 * gspec->getInitPosition("right_HandFingerThreeKnuckleTwoJoint");
for (std::vector<std::string >::const_iterator it = gc.getJointNames().begin(); it != gc.getJointNames().end(); it++) {
dart::dynamics::Joint *j = bh->getJoint((*it));
j->setActuatorType(dart::dynamics::Joint::FORCE);
j->setPositionLimited(true);
j->setPosition(0, joint_q_map[(*it)]);
}
int counter = 0;
while (ros::ok()) {
world->step(false);
for (std::map<std::string, double>::iterator it = joint_q_map.begin(); it != joint_q_map.end(); it++) {
dart::dynamics::Joint *j = bh->getJoint(it->first);
it->second = j->getPosition(0);
}
gc.controlStep(joint_q_map);
// Compute the joint forces needed to compensate for Coriolis forces and
// gravity
const Eigen::VectorXd& Cg = bh->getCoriolisAndGravityForces();
for (std::map<std::string, double>::iterator it = joint_q_map.begin(); it != joint_q_map.end(); it++) {
dart::dynamics::Joint *j = bh->getJoint(it->first);
int qidx = j->getIndexInSkeleton(0);
double u = gc.getControl(it->first);
double dq = j->getVelocity(0);
if (!gc.isBackdrivable(it->first)) {
j->setPositionLowerLimit(0, std::max(j->getPositionLowerLimit(0), it->second-0.01));
}
if (gc.isStopped(it->first)) {
j->setPositionLowerLimit(0, std::max(j->getPositionLowerLimit(0), it->second-0.01));
j->setPositionUpperLimit(0, std::min(j->getPositionUpperLimit(0), it->second+0.01));
// std::cout << it->first << " " << "stopped" << std::endl;
}
j->setForce(0, 0.02*(u-dq) + Cg(qidx));
}
for (int bidx = 0; bidx < bh->getNumBodyNodes(); bidx++) {
dart::dynamics::BodyNode *b = bh->getBodyNode(bidx);
const Eigen::Isometry3d &tf = b->getTransform();
KDL::Frame T_W_L;
EigenTfToKDL(tf, T_W_L);
// std::cout << b->getName() << std::endl;
publishTransform(br, T_W_L, b->getName(), "world");
}
int m_id = 0;
for (int bidx = 0; bidx < scene->getNumBodyNodes(); bidx++) {
dart::dynamics::BodyNode *b = scene->getBodyNode(bidx);
const Eigen::Isometry3d &tf = b->getTransform();
KDL::Frame T_W_L;
EigenTfToKDL(tf, T_W_L);
publishTransform(br, T_W_L, b->getName(), "world");
for (int cidx = 0; cidx < b->getNumCollisionShapes(); cidx++) {
dart::dynamics::ConstShapePtr sh = b->getCollisionShape(cidx);
if (sh->getShapeType() == dart::dynamics::Shape::MESH) {
std::shared_ptr<const dart::dynamics::MeshShape > msh = std::static_pointer_cast<const dart::dynamics::MeshShape >(sh);
m_id = markers_pub.addMeshMarker(m_id, KDL::Vector(), 0, 1, 0, 1, 1, 1, 1, msh->getMeshUri(), b->getName());
}
}
}
markers_pub.publish();
ros::spinOnce();
loop_rate.sleep();
counter++;
if (counter < 3000) {
}
else if (counter == 3000) {
dart::dynamics::Joint::Properties prop = bh->getJoint(0)->getJointProperties();
dart::dynamics::FreeJoint::Properties prop_free;
prop_free.mName = prop_free.mName;
prop_free.mT_ParentBodyToJoint = prop.mT_ParentBodyToJoint;
prop_free.mT_ChildBodyToJoint = prop.mT_ChildBodyToJoint;
prop_free.mIsPositionLimited = false;
prop_free.mActuatorType = dart::dynamics::Joint::VELOCITY;
bh->getRootBodyNode()->changeParentJointType<dart::dynamics::FreeJoint >(prop_free);
}
else if (counter < 4000) {
bh->getDof("Joint_pos_z")->setVelocity(-0.1);
}
else {
break;
}
}
//
// generate models
//
const std::string ob_name( "graspable" );
scene->getBodyNode(ob_name)->setFrictionCoeff(0.001);
// calculate point clouds for all links and for the grasped object
std::map<std::string, pcl::PointCloud<pcl::PointNormal>::Ptr > point_clouds_map;
std::map<std::string, pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr > point_pc_clouds_map;
std::map<std::string, KDL::Frame > frames_map;
std::map<std::string, boost::shared_ptr<std::vector<KDL::Frame > > > features_map;
std::map<std::string, boost::shared_ptr<pcl::VoxelGrid<pcl::PointNormal> > > grids_map;
for (int skidx = 0; skidx < world->getNumSkeletons(); skidx++) {
dart::dynamics::SkeletonPtr sk = world->getSkeleton(skidx);
for (int bidx = 0; bidx < sk->getNumBodyNodes(); bidx++) {
dart::dynamics::BodyNode *b = sk->getBodyNode(bidx);
const Eigen::Isometry3d &tf = b->getTransform();
const std::string &body_name = b->getName();
if (body_name.find("right_Hand") != 0 && body_name != ob_name) {
continue;
}
KDL::Frame T_W_L;
EigenTfToKDL(tf, T_W_L);
std::cout << body_name << " " << b->getNumCollisionShapes() << std::endl;
for (int cidx = 0; cidx < b->getNumCollisionShapes(); cidx++) {
dart::dynamics::ConstShapePtr sh = b->getCollisionShape(cidx);
if (sh->getShapeType() == dart::dynamics::Shape::MESH) {
std::shared_ptr<const dart::dynamics::MeshShape > msh = std::static_pointer_cast<const dart::dynamics::MeshShape >(sh);
std::cout << "mesh path: " << msh->getMeshPath() << std::endl;
std::cout << "mesh uri: " << msh->getMeshUri() << std::endl;
const Eigen::Isometry3d &tf = sh->getLocalTransform();
KDL::Frame T_L_S;
EigenTfToKDL(tf, T_L_S);
KDL::Frame T_S_L = T_L_S.Inverse();
const aiScene *sc = msh->getMesh();
if (sc->mNumMeshes != 1) {
std::cout << "ERROR: sc->mNumMeshes = " << sc->mNumMeshes << std::endl;
}
int midx = 0;
// std::cout << "v: " << sc->mMeshes[midx]->mNumVertices << " f: " << sc->mMeshes[midx]->mNumFaces << std::endl;
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_1 (new pcl::PointCloud<pcl::PointNormal>);
uniform_sampling(sc->mMeshes[midx], 1000000, *cloud_1);
for (int pidx = 0; pidx < cloud_1->points.size(); pidx++) {
KDL::Vector pt_L = T_L_S * KDL::Vector(cloud_1->points[pidx].x, cloud_1->points[pidx].y, cloud_1->points[pidx].z);
cloud_1->points[pidx].x = pt_L.x();
cloud_1->points[pidx].y = pt_L.y();
cloud_1->points[pidx].z = pt_L.z();
}
// Voxelgrid
boost::shared_ptr<pcl::VoxelGrid<pcl::PointNormal> > grid_(new pcl::VoxelGrid<pcl::PointNormal>);
pcl::PointCloud<pcl::PointNormal>::Ptr res(new pcl::PointCloud<pcl::PointNormal>);
grid_->setDownsampleAllData(true);
grid_->setSaveLeafLayout(true);
grid_->setInputCloud(cloud_1);
grid_->setLeafSize(0.004, 0.004, 0.004);
grid_->filter (*res);
point_clouds_map[body_name] = res;
frames_map[body_name] = T_W_L;
grids_map[body_name] = grid_;
std::cout << "res->points.size(): " << res->points.size() << std::endl;
pcl::search::KdTree<pcl::PointNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointNormal>);
// Setup the principal curvatures computation
pcl::PrincipalCurvaturesEstimation<pcl::PointNormal, pcl::PointNormal, pcl::PrincipalCurvatures> principalCurvaturesEstimation;
// Provide the original point cloud (without normals)
principalCurvaturesEstimation.setInputCloud (res);
// Provide the point cloud with normals
principalCurvaturesEstimation.setInputNormals(res);
// Use the same KdTree from the normal estimation
principalCurvaturesEstimation.setSearchMethod (tree);
principalCurvaturesEstimation.setRadiusSearch(0.02);
// Actually compute the principal curvatures
pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr principalCurvatures (new pcl::PointCloud<pcl::PrincipalCurvatures> ());
principalCurvaturesEstimation.compute (*principalCurvatures);
point_pc_clouds_map[body_name] = principalCurvatures;
features_map[body_name].reset( new std::vector<KDL::Frame >(res->points.size()) );
for (int pidx = 0; pidx < res->points.size(); pidx++) {
KDL::Vector nx, ny, nz(res->points[pidx].normal[0], res->points[pidx].normal[1], res->points[pidx].normal[2]);
if ( std::fabs( principalCurvatures->points[pidx].pc1 - principalCurvatures->points[pidx].pc2 ) > 0.001) {
nx = KDL::Vector(principalCurvatures->points[pidx].principal_curvature[0], principalCurvatures->points[pidx].principal_curvature[1], principalCurvatures->points[pidx].principal_curvature[2]);
}
else {
if (std::fabs(nz.z()) < 0.7) {
nx = KDL::Vector(0, 0, 1);
}
else {
nx = KDL::Vector(1, 0, 0);
}
}
ny = nz * nx;
nx = ny * nz;
nx.Normalize();
ny.Normalize();
nz.Normalize();
(*features_map[body_name])[pidx] = KDL::Frame( KDL::Rotation(nx, ny, nz), KDL::Vector(res->points[pidx].x, res->points[pidx].y, res->points[pidx].z) );
}
}
}
}
}
const double sigma_p = 0.01;//05;
const double sigma_q = 10.0/180.0*PI;//100.0;
const double sigma_r = 0.2;//05;
double sigma_c = 5.0/180.0*PI;
int m_id = 101;
// generate object model
boost::shared_ptr<ObjectModel > om(new ObjectModel);
for (int pidx = 0; pidx < point_clouds_map[ob_name]->points.size(); pidx++) {
if (point_pc_clouds_map[ob_name]->points[pidx].pc1 > 1.1 * point_pc_clouds_map[ob_name]->points[pidx].pc2) {
// e.g. pc1=1, pc2=0
// edge
om->addPointFeature((*features_map[ob_name])[pidx] * KDL::Frame(KDL::Rotation::RotZ(PI)), point_pc_clouds_map[ob_name]->points[pidx].pc1, point_pc_clouds_map[ob_name]->points[pidx].pc2);
om->addPointFeature((*features_map[ob_name])[pidx], point_pc_clouds_map[ob_name]->points[pidx].pc1, point_pc_clouds_map[ob_name]->points[pidx].pc2);
}
else {
for (double angle = 0.0; angle < 359.0/180.0*PI; angle += 20.0/180.0*PI) {
om->addPointFeature((*features_map[ob_name])[pidx] * KDL::Frame(KDL::Rotation::RotZ(angle)), point_pc_clouds_map[ob_name]->points[pidx].pc1, point_pc_clouds_map[ob_name]->points[pidx].pc2);
}
}
}
std::cout << "om.getPointFeatures().size(): " << om->getPointFeatures().size() << std::endl;
KDL::Frame T_W_O = frames_map[ob_name];
// generate collision model
std::map<std::string, std::list<std::pair<int, double> > > link_pt_map;
boost::shared_ptr<CollisionModel > cm(new CollisionModel);
cm->setSamplerParameters(sigma_p, sigma_q, sigma_r);
std::list<std::string > gripper_link_names;
for (int bidx = 0; bidx < bh->getNumBodyNodes(); bidx++) {
const std::string &link_name = bh->getBodyNode(bidx)->getName();
gripper_link_names.push_back(link_name);
}
double dist_range = 0.01;
for (std::list<std::string >::const_iterator nit = gripper_link_names.begin(); nit != gripper_link_names.end(); nit++) {
const std::string &link_name = (*nit);
if (point_clouds_map.find( link_name ) == point_clouds_map.end()) {
continue;
}
cm->addLinkContacts(dist_range, link_name, point_clouds_map[link_name], frames_map[link_name],
om->getPointFeatures(), T_W_O);
}
// generate hand configuration model
boost::shared_ptr<HandConfigurationModel > hm(new HandConfigurationModel);
std::map<std::string, double> joint_q_map_before( joint_q_map );
double angleDiffKnuckleTwo = 15.0/180.0*PI;
joint_q_map_before["right_HandFingerOneKnuckleTwoJoint"] -= angleDiffKnuckleTwo;
joint_q_map_before["right_HandFingerTwoKnuckleTwoJoint"] -= angleDiffKnuckleTwo;
joint_q_map_before["right_HandFingerThreeKnuckleTwoJoint"] -= angleDiffKnuckleTwo;
joint_q_map_before["right_HandFingerOneKnuckleThreeJoint"] -= angleDiffKnuckleTwo*0.333333;
joint_q_map_before["right_HandFingerTwoKnuckleThreeJoint"] -= angleDiffKnuckleTwo*0.333333;
joint_q_map_before["right_HandFingerThreeKnuckleThreeJoint"] -= angleDiffKnuckleTwo*0.333333;
hm->generateModel(joint_q_map_before, joint_q_map, 1.0, 10, sigma_c);
writeToXml(argv[3], cm, hm);
return 0;
}
void GraphNodeParameters::decrementGrav() {
grav(grav() - gravIncrement);
}
void GraphNodeParameters::incrementGrav() {
grav(grav() + gravIncrement);
}
void PhysicsComponent::applyGravity() {
Vec2 grav(0.0, gravity);
force += grav;
}
void
UZRectMollerup::tick (const GeometryRect& geo,
const std::vector<size_t>& drain_cell,
const double drain_water_level,
const Soil& soil,
SoilWater& soil_water, const SoilHeat& soil_heat,
const Surface& surface, const Groundwater& groundwater,
const double dt, Treelog& msg)
{
daisy_assert (K_average.get ());
const size_t edge_size = geo.edge_size (); // number of edges
const size_t cell_size = geo.cell_size (); // number of cells
// Insert magic here.
ublas::vector<double> Theta (cell_size); // water content
ublas::vector<double> Theta_previous (cell_size); // at start of small t-step
ublas::vector<double> h (cell_size); // matrix pressure
ublas::vector<double> h_previous (cell_size); // at start of small timestep
ublas::vector<double> h_ice (cell_size); //
ublas::vector<double> S (cell_size); // sink term
ublas::vector<double> S_vol (cell_size); // sink term
#ifdef TEST_OM_DEN_ER_BRUGT
ublas::vector<double> S_macro (cell_size); // sink term
std::vector<double> S_drain (cell_size, 0.0); // matrix-> macro -> drain flow
std::vector<double> S_drain_sum (cell_size, 0.0); // For large timestep
const std::vector<double> S_matrix (cell_size, 0.0); // matrix -> macro
std::vector<double> S_matrix_sum (cell_size, 0.0); // for large timestep
#endif
ublas::vector<double> T (cell_size); // temperature
ublas::vector<double> Kold (edge_size); // old hydraulic conductivity
ublas::vector<double> Ksum (edge_size); // Hansen hydraulic conductivity
ublas::vector<double> Kcell (cell_size); // hydraulic conductivity
ublas::vector<double> Kold_cell (cell_size); // old hydraulic conductivity
ublas::vector<double> Ksum_cell (cell_size); // Hansen hydraulic conductivity
ublas::vector<double> h_lysimeter (cell_size);
std::vector<bool> active_lysimeter (cell_size);
const std::vector<size_t>& edge_above = geo.cell_edges (Geometry::cell_above);
const size_t edge_above_size = edge_above.size ();
ublas::vector<double> remaining_water (edge_above_size);
std::vector<bool> drain_cell_on (drain_cell.size (),false);
for (size_t i = 0; i < edge_above_size; i++)
{
const size_t edge = edge_above[i];
remaining_water (i) = surface.h_top (geo, edge);
}
ublas::vector<double> q; // Accumulated flux
q = ublas::zero_vector<double> (edge_size);
ublas::vector<double> dq (edge_size); // Flux in small timestep.
dq = ublas::zero_vector<double> (edge_size);
//Make Qmat area diagonal matrix
//Note: This only needs to be calculated once...
ublas::banded_matrix<double> Qmat (cell_size, cell_size, 0, 0);
for (int c = 0; c < cell_size; c++)
Qmat (c, c) = geo.cell_volume (c);
// make vectors
for (size_t cell = 0; cell != cell_size ; ++cell)
{
Theta (cell) = soil_water.Theta (cell);
h (cell) = soil_water.h (cell);
h_ice (cell) = soil_water.h_ice (cell);
S (cell) = soil_water.S_sum (cell);
S_vol (cell) = S (cell) * geo.cell_volume (cell);
if (use_forced_T)
T (cell) = forced_T;
else
T (cell) = soil_heat.T (cell);
h_lysimeter (cell) = geo.zplus (cell) - geo.cell_z (cell);
}
// Remember old value.
Theta_error = Theta;
// Start time loop
double time_left = dt; // How much of the large time step left.
double ddt = dt; // We start with small == large time step.
int number_of_time_step_reductions = 0;
int iterations_with_this_time_step = 0;
int n_small_time_steps = 0;
while (time_left > 0.0)
{
if (ddt > time_left)
ddt = time_left;
std::ostringstream tmp_ddt;
tmp_ddt << "Time t = " << (dt - time_left)
<< "; ddt = " << ddt
<< "; steps " << n_small_time_steps
<< "; time left = " << time_left;
Treelog::Open nest (msg, tmp_ddt.str ());
if (n_small_time_steps > 0
&& (n_small_time_steps%msg_number_of_small_time_steps) == 0)
{
msg.touch ();
msg.flush ();
}
n_small_time_steps++;
if (n_small_time_steps > max_number_of_small_time_steps)
{
msg.debug ("Too many small timesteps");
throw "Too many small timesteps";
}
// Initialization for each small time step.
if (debug > 0)
{
std::ostringstream tmp;
tmp << "h = " << h << "\n";
tmp << "Theta = " << Theta;
msg.message (tmp.str ());
}
int iterations_used = 0;
h_previous = h;
Theta_previous = Theta;
if (debug == 5)
{
std::ostringstream tmp;
tmp << "Remaining water at start: " << remaining_water;
msg.message (tmp.str ());
}
ublas::vector<double> h_conv;
for (size_t cell = 0; cell != cell_size ; ++cell)
active_lysimeter[cell] = h (cell) > h_lysimeter (cell);
for (size_t edge = 0; edge != edge_size ; ++edge)
{
Kold[edge] = find_K_edge (soil, geo, edge, h, h_ice, h_previous, T);
Ksum [edge] = 0.0;
}
std::vector<top_state> state (edge_above.size (), top_undecided);
// We try harder with smaller timesteps.
const int max_loop_iter
= max_iterations * (number_of_time_step_reductions
* max_iterations_timestep_reduction_factor + 1);
do // Start iteration loop
{
h_conv = h;
iterations_used++;
std::ostringstream tmp_conv;
tmp_conv << "Convergence " << iterations_used;
Treelog::Open nest (msg, tmp_conv.str ());
if (debug == 7)
msg.touch ();
// Calculate conductivity - The Hansen method
for (size_t e = 0; e < edge_size; e++)
{
Ksum[e] += find_K_edge (soil, geo, e, h, h_ice, h_previous, T);
Kedge[e] = (Ksum[e] / (iterations_used + 0.0)+ Kold[e]) / 2.0;
}
//Initialize diffusive matrix
Solver::Matrix diff (cell_size);
// diff = ublas::zero_matrix<double> (cell_size, cell_size);
diffusion (geo, Kedge, diff);
//Initialize gravitational matrix
ublas::vector<double> grav (cell_size); //ublass compatibility
grav = ublas::zero_vector<double> (cell_size);
gravitation (geo, Kedge, grav);
// Boundary matrices and vectors
ublas::banded_matrix<double> Dm_mat (cell_size, cell_size,
0, 0); // Dir bc
Dm_mat = ublas::zero_matrix<double> (cell_size, cell_size);
ublas::vector<double> Dm_vec (cell_size); // Dir bc
Dm_vec = ublas::zero_vector<double> (cell_size);
ublas::vector<double> Gm (cell_size); // Dir bc
Gm = ublas::zero_vector<double> (cell_size);
ublas::vector<double> B (cell_size); // Neu bc
B = ublas::zero_vector<double> (cell_size);
lowerboundary (geo, groundwater, active_lysimeter, h,
Kedge,
dq, Dm_mat, Dm_vec, Gm, B, msg);
upperboundary (geo, soil, T, surface, state, remaining_water, h,
Kedge,
dq, Dm_mat, Dm_vec, Gm, B, ddt, debug, msg, dt);
Darcy (geo, Kedge, h, dq); //for calculating drain fluxes
//Initialize water capacity matrix
ublas::banded_matrix<double> Cw (cell_size, cell_size, 0, 0);
for (size_t c = 0; c < cell_size; c++)
Cw (c, c) = soil.Cw2 (c, h[c]);
std::vector<double> h_std (cell_size);
//ublas vector -> std vector
std::copy(h.begin (), h.end (), h_std.begin ());
#ifdef TEST_OM_DEN_ER_BRUGT
for (size_t cell = 0; cell != cell_size ; ++cell)
{
S_macro (cell) = (S_matrix[cell] + S_drain[cell])
* geo.cell_volume (cell);
}
#endif
//Initialize sum matrix
Solver::Matrix summat (cell_size);
noalias (summat) = diff + Dm_mat;
//Initialize sum vector
ublas::vector<double> sumvec (cell_size);
sumvec = grav + B + Gm + Dm_vec - S_vol
#ifdef TEST_OM_DEN_ER_BRUGT
- S_macro
#endif
;
// QCw is shorthand for Qmatrix * Cw
Solver::Matrix Q_Cw (cell_size);
noalias (Q_Cw) = prod (Qmat, Cw);
//Initialize A-matrix
Solver::Matrix A (cell_size);
noalias (A) = (1.0 / ddt) * Q_Cw - summat;
// Q_Cw_h is shorthand for Qmatrix * Cw * h
const ublas::vector<double> Q_Cw_h = prod (Q_Cw, h);
//Initialize b-vector
ublas::vector<double> b (cell_size);
//b = sumvec + (1.0 / ddt) * (Qmatrix * Cw * h + Qmatrix *(Wxx-Wyy));
b = sumvec + (1.0 / ddt) * (Q_Cw_h
+ prod (Qmat, Theta_previous-Theta));
// Force active drains to zero h.
drain (geo, drain_cell, drain_water_level,
h, Theta_previous, Theta, S_vol,
#ifdef TEST_OM_DEN_ER_BRUGT
S_macro,
#endif
dq, ddt, drain_cell_on, A, b, debug, msg);
try {
solver->solve (A, b, h); // Solve Ah=b with regard to h.
} catch (const char *const error) {
std::ostringstream tmp;
tmp << "Could not solve equation system: " << error;
msg.warning (tmp.str ());
// Try smaller timestep.
iterations_used = max_loop_iter + 100;
break;
}
for (int c=0; c < cell_size; c++) // update Theta
Theta (c) = soil.Theta (c, h (c), h_ice (c));
if (debug > 1)
{
std::ostringstream tmp;
tmp << "Time left = " << time_left << ", ddt = " << ddt
<< ", iteration = " << iterations_used << "\n";
tmp << "B = " << B << "\n";
tmp << "h = " << h << "\n";
tmp << "Theta = " << Theta;
msg.message (tmp.str ());
}
for (int c=0; c < cell_size; c++)
{
if (h (c) < min_pressure_potential || h (c) > max_pressure_potential)
{
std::ostringstream tmp;
tmp << "Pressure potential out of realistic range, h["
<< c << "] = " << h (c);
msg.debug (tmp.str ());
iterations_used = max_loop_iter + 100;
break;
}
}
}
while (!converges (h_conv, h) && iterations_used <= max_loop_iter);
if (iterations_used > max_loop_iter)
{
number_of_time_step_reductions++;
if (number_of_time_step_reductions > max_time_step_reductions)
{
msg.debug ("Could not find solution");
throw "Could not find solution";
}
iterations_with_this_time_step = 0;
ddt /= time_step_reduction;
h = h_previous;
Theta = Theta_previous;
}
else
{
// Update dq for new h.
ublas::banded_matrix<double> Dm_mat (cell_size, cell_size,
0, 0); // Dir bc
Dm_mat = ublas::zero_matrix<double> (cell_size, cell_size);
ublas::vector<double> Dm_vec (cell_size); // Dir bc
Dm_vec = ublas::zero_vector<double> (cell_size);
ublas::vector<double> Gm (cell_size); // Dir bc
Gm = ublas::zero_vector<double> (cell_size);
ublas::vector<double> B (cell_size); // Neu bc
B = ublas::zero_vector<double> (cell_size);
lowerboundary (geo, groundwater, active_lysimeter, h,
Kedge,
dq, Dm_mat, Dm_vec, Gm, B, msg);
upperboundary (geo, soil, T, surface, state, remaining_water, h,
Kedge,
dq, Dm_mat, Dm_vec, Gm, B, ddt, debug, msg, dt);
Darcy (geo, Kedge, h, dq);
#ifdef TEST_OM_DEN_ER_BRUGT
// update macropore flow components
for (int c = 0; c < cell_size; c++)
{
S_drain_sum[c] += S_drain[c] * ddt/dt;
S_matrix_sum[c] += S_matrix[c] * ddt/dt;
}
#endif
std::vector<double> h_std_new (cell_size);
std::copy(h.begin (), h.end (), h_std_new.begin ());
// Update remaining_water.
for (size_t i = 0; i < edge_above.size (); i++)
{
const int edge = edge_above[i];
const int cell = geo.edge_other (edge, Geometry::cell_above);
const double out_sign = (cell == geo.edge_from (edge))
? 1.0 : -1.0;
remaining_water[i] += out_sign * dq (edge) * ddt;
daisy_assert (std::isfinite (dq (edge)));
}
if (debug == 5)
{
std::ostringstream tmp;
tmp << "Remaining water at end: " << remaining_water;
msg.message (tmp.str ());
}
// Contribution to large time step.
daisy_assert (std::isnormal (dt));
daisy_assert (std::isnormal (ddt));
q += dq * ddt / dt;
for (size_t e = 0; e < edge_size; e++)
{
daisy_assert (std::isfinite (dq (e)));
daisy_assert (std::isfinite (q (e)));
}
for (size_t e = 0; e < edge_size; e++)
{
daisy_assert (std::isfinite (dq (e)));
daisy_assert (std::isfinite (q (e)));
}
time_left -= ddt;
iterations_with_this_time_step++;
if (iterations_with_this_time_step > time_step_reduction)
{
number_of_time_step_reductions--;
iterations_with_this_time_step = 0;
ddt *= time_step_reduction;
}
}
// End of small time step.
}
// Mass balance.
// New = Old - S * dt + q_in * dt - q_out * dt + Error =>
// 0 = Old - New - S * dt + q_in * dt - q_out * dt + Error
Theta_error -= Theta; // Old - New
Theta_error -= S * dt;
#ifdef TEST_OM_DEN_ER_BRUGT
for (size_t c = 0; c < cell_size; c++)
Theta_error (c) -= (S_matrix_sum[c] + S_drain_sum[c]) * dt;
#endif
for (size_t edge = 0; edge != edge_size; ++edge)
{
const int from = geo.edge_from (edge);
const int to = geo.edge_to (edge);
const double flux = q (edge) * geo.edge_area (edge) * dt;
if (geo.cell_is_internal (from))
Theta_error (from) -= flux / geo.cell_volume (from);
if (geo.cell_is_internal (to))
Theta_error (to) += flux / geo.cell_volume (to);
}
// Find drain sink from mass balance.
#ifdef TEST_OM_DEN_ER_BRUGT
std::fill(S_drain.begin (), S_drain.end (), 0.0);
#else
std::vector<double> S_drain (cell_size);
#endif
for (size_t i = 0; i < drain_cell.size (); i++)
{
const size_t cell = drain_cell[i];
S_drain[cell] = Theta_error (cell) / dt;
Theta_error (cell) -= S_drain[cell] * dt;
}
if (debug == 2)
{
double total_error = 0.0;
double total_abs_error = 0.0;
double max_error = 0.0;
int max_cell = -1;
for (size_t cell = 0; cell != cell_size; ++cell)
{
const double volume = geo.cell_volume (cell);
const double error = Theta_error (cell);
total_error += volume * error;
total_abs_error += std::fabs (volume * error);
if (std::fabs (error) > std::fabs (max_error))
{
max_error = error;
max_cell = cell;
}
}
std::ostringstream tmp;
tmp << "Total error = " << total_error << " [cm^3], abs = "
<< total_abs_error << " [cm^3], max = " << max_error << " [] in cell "
<< max_cell;
msg.message (tmp.str ());
}
// Make it official.
for (size_t cell = 0; cell != cell_size; ++cell)
soil_water.set_content (cell, h (cell), Theta (cell));
#ifdef TEST_OM_DEN_ER_BRUGT
soil_water.add_tertiary_sink (S_matrix_sum);
soil_water.drain (S_drain_sum, msg);
#endif
for (size_t edge = 0; edge != edge_size; ++edge)
{
daisy_assert (std::isfinite (q[edge]));
soil_water.set_flux (edge, q[edge]);
}
soil_water.drain (S_drain, msg);
// End of large time step.
}
/* Returns a vector of length N corresponding to each link's qdd */
std::vector<double>
ckbot::chain_rate::base_tip_step(std::vector<double> s, std::vector<double> T)
{
int N = c.num_links();
std::vector<double> q(N);
std::vector<double> qd(N);
std::vector<double> qdd(N);
for(int i=0; i<N; ++i)
{
q[i] = s[i];
qd[i] = s[N+i];
}
Eigen::VectorXd grav(6);
grav << 0,0,0,0,0,9.81;
Eigen::Matrix3d R_cur;
Eigen::Vector3d r_i_ip;
Eigen::MatrixXd phi(6,6);
Eigen::VectorXd alpha(6);
alpha = Eigen::VectorXd::Zero(6);
Eigen::VectorXd alpha_p(6);
Eigen::VectorXd G(6);
Eigen::VectorXd a(6);
Eigen::VectorXd H_b_frame_star(6);
Eigen::VectorXd H_w_frame_star(6);
Eigen::RowVectorXd H(6);
for (int i=0; i<N; ++i)
{
module_link& cur = c.get_link(i);
/* 3x3 Rotation matrix from this link's coordinate
* frame to the world frame
*/
R_cur = c.get_current_R(i);
/* Vector, in world frame, from this link's inbound joint to its
* outbound joint
*/
r_i_ip = R_cur*(cur.get_r_ip1() - cur.get_r_im1());
/* 6x6 Spatial body operator matrix that transforms spatial
* vectors from the outbound joint to the inbound joint in
* the world frame
*/
phi = get_body_trans(r_i_ip);
/* Transform the spatial accel vector from the inbound
* link's inbound joint to this link's inbound joint
*/
alpha_p = phi.transpose()*alpha;
/* These are calculated in tip_base_step */
int cur_index = 0;
for (int k = (6*i); k <= (6*(i+1)-1); ++k, ++cur_index)
{
G[cur_index] = G_all[k];
a[cur_index] = a_all[k];
}
/* The angular acceleration of this link's joint */
/* TODO: 6DOF changes here...This gets a whoooole lotta broken */
qdd[i] = mu_all[i] - G.transpose()*alpha_p;
/* Joint matrix in body frame. Check tip_base_step for the definition */
/* TODO: 6DOF changes here a plenty */
H_b_frame_star = cur.get_joint_matrix().transpose();
Eigen::MatrixXd tmp_6x6(6,6);
tmp_6x6 << R_cur, Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), R_cur;
/* Joint matrix in the world frame */
H_w_frame_star = tmp_6x6*H_b_frame_star;
/* As a row vector */
H = H_w_frame_star.transpose();
/* Spatial acceleration of this link at its inbound joint in
* its frame (ie: including its joint accel)
*/
alpha = alpha_p + H.transpose()*qdd[i] + a;
}
return qdd;
}
void PhysicsServerSharedMemory::processClientCommands()
{
if (m_data->m_isConnected && m_data->m_testBlock1)
{
///we ignore overflow of integer for now
if (m_data->m_testBlock1->m_numClientCommands> m_data->m_testBlock1->m_numProcessedClientCommands)
{
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
btAssert(m_data->m_testBlock1->m_numClientCommands==m_data->m_testBlock1->m_numProcessedClientCommands+1);
const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
m_data->m_testBlock1->m_numProcessedClientCommands++;
//no timestamp yet
int timeStamp = 0;
//consume the command
switch (clientCmd.m_type)
{
case CMD_SEND_BULLET_DATA_STREAM:
{
b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength);
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
m_data->m_worldImporters.push_back(worldImporter);
bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength);
if (completedOk)
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
m_data->submitServerStatus(status);
} else
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
}
break;
}
case CMD_LOAD_URDF:
{
//at the moment, we only load 1 urdf / robot
if (m_data->m_urdfLinkNameMapper.size())
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
break;
}
const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments;
b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName);
btAssert((clientCmd.m_updateFlags&URDF_ARGS_FILE_NAME) !=0);
btAssert(urdfArgs.m_urdfFileName);
btVector3 initialPos(0,0,0);
btQuaternion initialOrn(0,0,0,1);
if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION)
{
initialPos[0] = urdfArgs.m_initialPosition[0];
initialPos[1] = urdfArgs.m_initialPosition[1];
initialPos[2] = urdfArgs.m_initialPosition[2];
}
if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION)
{
initialOrn[0] = urdfArgs.m_initialOrientation[0];
initialOrn[1] = urdfArgs.m_initialOrientation[1];
initialOrn[2] = urdfArgs.m_initialOrientation[2];
initialOrn[3] = urdfArgs.m_initialOrientation[3];
}
bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? urdfArgs.m_useMultiBody : true;
bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? urdfArgs.m_useFixedBase: false;
//load the actual URDF and send a report: completed or failed
bool completedOk = loadUrdf(urdfArgs.m_urdfFileName,
initialPos,initialOrn,
useMultiBody, useFixedBase);
SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
if (completedOk)
{
if (m_data->m_urdfLinkNameMapper.size())
{
serverCmd.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
}
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
} else
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
}
break;
}
case CMD_CREATE_SENSOR:
{
b3Printf("Processed CMD_CREATE_SENSOR");
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
btAssert(mb);
for (int i=0;i<clientCmd.m_createSensorArguments.m_numJointSensorChanges;i++)
{
int jointIndex = clientCmd.m_createSensorArguments.m_jointIndex[i];
if (clientCmd.m_createSensorArguments.m_enableJointForceSensor[i])
{
if (mb->getLink(jointIndex).m_jointFeedback)
{
b3Warning("CMD_CREATE_SENSOR: sensor for joint [%d] already enabled", jointIndex);
} else
{
btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
fb->m_reactionForces.setZero();
mb->getLink(jointIndex).m_jointFeedback = fb;
m_data->m_multiBodyJointFeedbacks.push_back(fb);
};
} else
{
if (mb->getLink(jointIndex).m_jointFeedback)
{
m_data->m_multiBodyJointFeedbacks.remove(mb->getLink(jointIndex).m_jointFeedback);
delete mb->getLink(jointIndex).m_jointFeedback;
mb->getLink(jointIndex).m_jointFeedback=0;
} else
{
b3Warning("CMD_CREATE_SENSOR: cannot perform sensor removal request, no sensor on joint [%d]", jointIndex);
};
}
}
} else
{
b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet");
}
#if 0
//todo(erwincoumans) here is some sample code to hook up a force/torque sensor for btTypedConstraint/btRigidBody
/*
for (int i=0;i<m_data->m_dynamicsWorld->getNumConstraints();i++)
{
btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i);
btJointFeedback* fb = new btJointFeedback();
m_data->m_jointFeedbacks.push_back(fb);
c->setJointFeedback(fb);
}
*/
#endif
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
case CMD_SEND_DESIRED_STATE:
{
b3Printf("Processed CMD_SEND_DESIRED_STATE");
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
btAssert(mb);
switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
{
case CONTROL_MODE_TORQUE:
{
b3Printf("Using CONTROL_MODE_TORQUE");
mb->clearForcesAndTorques();
int torqueIndex = 0;
btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]);
btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]);
torqueIndex+=6;
mb->addBaseForce(f);
mb->addBaseTorque(t);
for (int link=0;link<mb->getNumLinks();link++)
{
for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
{
double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
mb->addJointTorqueMultiDof(link,dof,torque);
torqueIndex++;
}
}
break;
}
case CONTROL_MODE_VELOCITY:
{
b3Printf("Using CONTROL_MODE_VELOCITY");
int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base
for (int link=0;link<mb->getNumLinks();link++)
{
if (supportsJointMotor(mb,link))
{
btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link];
if (motorPtr)
{
btMultiBodyJointMotor* motor = *motorPtr;
btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
motor->setVelocityTarget(desiredVelocity);
btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
}
}
dofIndex += mb->getLink(link).m_dofCount;
}
}
break;
}
default:
{
b3Printf("m_controlMode not implemented yet");
break;
}
}
}
SharedMemoryStatus& status = m_data->createServerStatus(CMD_DESIRED_STATE_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
b3Printf("Sending the actual state (Q,U)");
if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0;
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
//always add the base, even for static (non-moving objects)
//so that we can easily move the 'fixed' base when needed
//do we don't use this conditional "if (!mb->hasFixedBase())"
{
btTransform tr;
tr.setOrigin(mb->getBasePos());
tr.setRotation(mb->getWorldToBaseRot().inverse());
//base position in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ +=7;//pos + quaternion
//base linear velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2];
//base angular velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2];
totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
}
for (int l=0;l<mb->getNumLinks();l++)
{
for (int d=0;d<mb->getLink(l).m_posVarCount;d++)
{
serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
}
for (int d=0;d<mb->getLink(l).m_dofCount;d++)
{
serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
}
if (0 == mb->getLink(l).m_jointFeedback)
{
for (int d=0;d<6;d++)
{
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0;
}
} else
{
btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear();
btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear();
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2];
}
}
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
m_data->submitServerStatus(serverCmd);
} else
{
b3Warning("Request state but no multibody available");
SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
}
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
b3Printf("Step simulation request");
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime);
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_STEP_FORWARD_SIMULATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
{
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY)
{
btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0],
clientCmd.m_physSimParamArgs.m_gravityAcceleration[1],
clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]);
this->m_data->m_dynamicsWorld->setGravity(grav);
b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]);
}
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break;
};
case CMD_INIT_POSE:
{
b3Printf("Server Init Pose not implemented yet");
///@todo: implement this
m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0));
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
case CMD_RESET_SIMULATION:
{
//clean up all data
m_data->m_guiHelper->getRenderInterface()->removeAllInstances();
deleteDynamicsWorld();
createEmptyDynamicsWorld();
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
btVector3 halfExtents(1,1,1);
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS)
{
halfExtents = btVector3(
clientCmd.m_createBoxShapeArguments.m_halfExtentsX,
clientCmd.m_createBoxShapeArguments.m_halfExtentsY,
clientCmd.m_createBoxShapeArguments.m_halfExtentsZ);
}
btTransform startTrans;
startTrans.setIdentity();
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION)
{
startTrans.setOrigin(btVector3(
clientCmd.m_createBoxShapeArguments.m_initialPosition[0],
clientCmd.m_createBoxShapeArguments.m_initialPosition[1],
clientCmd.m_createBoxShapeArguments.m_initialPosition[2]));
}
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION)
{
b3Printf("test\n");
startTrans.setRotation(btQuaternion(
clientCmd.m_createBoxShapeArguments.m_initialOrientation[0],
clientCmd.m_createBoxShapeArguments.m_initialOrientation[1],
clientCmd.m_createBoxShapeArguments.m_initialOrientation[2],
clientCmd.m_createBoxShapeArguments.m_initialOrientation[3]));
}
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
m_data->m_worldImporters.push_back(worldImporter);
btCollisionShape* shape = worldImporter->createBoxShape(halfExtents);
btScalar mass = 0.f;
bool isDynamic = (mass>0);
worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0);
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break;
}
default:
{
b3Error("Unknown command encountered");
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_UNKNOWN_COMMAND_FLUSHED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
}
};
}
}
}
void
ckbot::chain_rate::tip_base_step(std::vector<double> s, std::vector<double> T)
{
int N = c.num_links();
std::vector<double> q(N);
std::vector<double> qd(N);
for(int i=0; i<N; ++i)
{
q[i] = s[i];
qd[i] = s[N+i];
}
Eigen::Matrix3d Iden = Eigen::Matrix3d::Identity();
Eigen::Matrix3d Zero = Eigen::Matrix3d::Zero();
/* Declare and initialized all of the loop variables */
/* TODO: Can we allocate all of this on the heap *once* for a particular
* rate object, and then just use pointers to them after? Would this be fast?
* Re-initializing these every time through here has to be slow...
*/
Eigen::MatrixXd pp(6,6);
pp = Eigen::MatrixXd::Zero(6,6);
Eigen::VectorXd zp(6);
zp << 0,0,0,0,0,0;
Eigen::VectorXd grav(6);
grav << 0,0,0,0,0,9.81;
Eigen::Matrix3d L_oc_tilde;
Eigen::Matrix3d R_cur;
Eigen::MatrixXd M_cur(6,6);
Eigen::MatrixXd M_cm(6,6);
M_cm = Eigen::MatrixXd::Zero(6,6);
Eigen::Matrix3d J_o;
Eigen::Vector3d r_i_ip(0,0,0);
Eigen::Vector3d r_i_cm(0,0,0);
Eigen::MatrixXd phi(6,6);
Eigen::MatrixXd phi_cm(6,6);
Eigen::MatrixXd p_cur(6,6);
Eigen::VectorXd H_b_frame_star(6);
Eigen::VectorXd H_w_frame_star(6);
Eigen::RowVectorXd H(6);
double D = 0.0;
Eigen::VectorXd G(6);
Eigen::MatrixXd tau_tilde(6,6);
Eigen::Vector3d omega(0,0,0);
Eigen::Matrix3d omega_cross;
Eigen::VectorXd a(6);
Eigen::VectorXd b(6);
Eigen::VectorXd z(6);
double C = 0.0;
double CF = 0.0;
double SPOLES = 12.0;
double RPOLES = 14.0;
double epsilon = 0.0;
double mu = 0.0;
/* Recurse from the tip to the base of this chain */
for (int i = N-1; i >= 0; --i)
{
module_link& cur = c.get_link(i);
/* Rotation from the world to this link */
R_cur = c.get_current_R(i);
/* Vector, in world frame, from inbound joint to outbound joint of
* this link
*/
r_i_ip = R_cur*(cur.get_r_ip1() - cur.get_r_im1());
/* Operator to transform spatial vectors from outbound joint
* to the inbound joint
*/
phi = get_body_trans(r_i_ip);
/* Vector from the inbound joint to the center of mass */
r_i_cm = R_cur*(-cur.get_r_im1());
/* Operator to transform spatial vectors from the center of mass
* to the inbound joint
*/
phi_cm = get_body_trans(r_i_cm);
/* Cross product matrix corresponding to the vector from
* the inbound joint to the center of mass
*/
L_oc_tilde = get_cross_mat(r_i_cm);
/* 3x3 Inertia matrix of this link about its inbound joint and wrt the
* world coordinate system.
*
* NOTE: Similarity transform of get_I_cm() because it is wrt
* the module frame
*/
J_o = R_cur*cur.get_I_cm()*R_cur.transpose() - cur.get_mass()*L_oc_tilde*L_oc_tilde;
/* Spatial mass matrix of this link about its inbound joint and wrt the
* world coordinate system.
*
*/
M_cur << J_o, cur.get_mass()*L_oc_tilde,
-cur.get_mass()*L_oc_tilde, cur.get_mass()*Iden;
/* Spatial mass matrix of this link about its cm and wrt the
* world coordinate system.
*
* NOTE: Similarity transform of get_I_cm() because it is wrt
* the module frame
*/
M_cm << R_cur*cur.get_I_cm()*R_cur.transpose(), Zero,
Zero, cur.get_mass()*Iden;
/* Articulated Body Spatial inertia matrix of the links from
* this one all the way to the tip of the chain (accounting for
* articulated body spatial inertia that gets through each joint)
*
* pp is p_cur from the previous (outbound) link. This is the
* zero vector when we are on the farthest outbound link (the tip)
*
* In much the same way that we use a similarity transform to change
* the frame of the inertia matrix, a similarity transform moves
* pp from the previous module's base joint to this module's base joint.
*/
p_cur = phi*pp*phi.transpose() + M_cur;
/* The joint matrix, in the body frame, that maps the change
* in general coordinates relating this link to the next inward link.
*
* It essentially defines what "gets through" a link. IE:
* H_b_frame_star = [0,0,1,0,0,0] means that movements and forces
* in the joint's rotational Z axis get through. This is a single
* DOF rotational joint.
*
* H_b_frame_star = eye(6) means that forces in all 6
* (3 rotational, 3 linear) get through the joint using
* 6 independent general coordinates. This is a
* free 6DOF joint.
*
* H_b_frame_star = [0,0,2,0,0,1] is helical joint that rotates twice
* for every single linear unit moved. It is controlled by a single
* general coordinate.
*
* (NOTE/TODO: Using anything but [0,0,1,0,0,0] breaks the code right now.)
*/
H_b_frame_star = cur.get_joint_matrix().transpose();
/* To transform a spatial vector (6 x 1) from one coordinate
* system to another, multiply it by the 6x6 matrix with
* the rotation matrix form one frame to the other on the diagonals
*/
Eigen::MatrixXd tmp_6x6(6,6);
tmp_6x6 << R_cur, Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), R_cur;
/* Joint matrix in the world frame */
H_w_frame_star = tmp_6x6*H_b_frame_star;
/* As a row vector */
H = H_w_frame_star.transpose();
D = H*p_cur*H.transpose(); /* TODO:Could use H_w_frame_star..but..clarity */
/* TODO: This needs to be changed to D.inverse() for 6DOF
* and D needs to be made a variable sized Eigen Matrix
*/
G = p_cur*H.transpose()*(1.0/D);
/* Articulated body projection operator through this joint. This defines
* which part of the articulated body spatial inertia gets through this joint
*/
tau_tilde = Eigen::MatrixXd::Identity(6,6) - G*H;
/* pp for the next time around the loop */
pp = tau_tilde*p_cur;
omega = c.get_angular_velocity(i);
omega_cross = get_cross_mat(omega);
/* Gyroscopic force */
b << omega_cross*J_o*omega,
cur.get_mass()*omega_cross*omega_cross*r_i_cm;
/* Coriolis and centripetal accel */
a << 0,0,0,
omega_cross*omega_cross*r_i_cm;
/* z is the total spatial force seen by this link at its inward joint
* phi*zp = Force through joint from tip-side module
* b = Gyroscopic force (velocity dependent)
* p_cur*a = Coriolis and Centripetal forces
* phi_cm*M_cm*grav = Force of grav at CM transformed to inbound jt
*/
z = phi*zp + b + p_cur*a + phi_cm*M_cm*grav;
/* Velocity related damping force in the joint */
/* TODO: 6DOF change. epsilon will be an matrix when the joint
* matrix is not a row matrix, so using C in the calculation
* of epsilon will break things.
*/
C = -cur.get_damping()*qd[i];
/* Cogging force which is a function of the joint angle and
* the motor specifics.
*/
CF = cur.get_rotor_cogging()*sin(RPOLES*(q[i]-cur.get_cogging_offset()))
+ cur.get_stator_cogging()*sin(SPOLES*(q[i]-cur.get_cogging_offset()));
/* The "Effective" force through the joint that can actually create
* accelerations. IE: Forces that are in directions in which the joint
* can actually move.
*/
/* TODO: 6DOF change. Epsilon will not be 1x1 for a 6DOF joint, so
* both T[i] and C used here as they are will break things.
*/
epsilon = T[i] + C + CF - H*z;
mu = (1/D)*epsilon; /* 6DOF change: D will be a matrix, need to invert it */
zp = z + G*epsilon;
/* base_tip_step needs G, a, and mu. So store them in the chain object.*/
mu_all[i] = mu;
int cur_index=0;
for (int k=(6*i); k <= (6*(i+1)-1); ++k,++cur_index)
{
G_all[k] = G[cur_index];
a_all[k] = a[cur_index];
}
}
}
