I have some C++ code in which i give the turtlebot a coordinate using:
goal.target_pose.pose.position.x = mypoint.point.x
goal.target_pose.pose.position.y = mypoint.point.y
The robot moves from one point to another. However only rarely does it do this in a straight line. It seems as though the turtlebot starts moving before it is done rotating. There aren't any obstacles in between the points. It is able to get to every point, but it tends to move in small arc between them.
Any ideas on how to force the robot to move in a straight line, when there are no obstacles in its way?
Edit: I use turtlebot_bringup minimal.launch and turtlebot_navigation gmapping_demo.launch.
It actually is what Dave suggested, that the wheels are moving at different speeds and a two-wheeled robot will follow an arc around the wheel that is moving slower. But this is not because of variations of the wheel diameter, but because no two motors are identical (due to manufacturing) and therefore even the motors of the same model will have slight variations. Given the same current and voltage, the motors will drive at a slightly different speed; which makes the robot follow an arc.
The solution is in implement a feedback control, where you get the rotation feedback and implement the error in a simple control scheme such as PID control. Pseudocode for PID is as follows:
Kp = .. //constant
Loop forever
read sensors
error = TargetValue - offset
integral = integral + error // calculate the integral
derivative = error - lastError // calculate the derivative
Turn = Kp*error + Ki*integral + Kd*derivative
lastError = error // save the current error so it can be the lastError next time around
end loop forever
Related
Assume we have the following pseudo-code:
class Position{
int x, y
}
class Body{
Body parent;
Position start; //always initialized relative to parent's
}
class OrbitingBody extends Body{
int angularVelocity; // angles travelled per day
int radius; // radius of the orbit, like earths orbit radius around sun
//returns its position after 'days' days relative to its parent
Position getRelativePosition(int days) {
totalAngles = self.angularVelocity * days
roundedAngle = totalAngles % 360
return Math.polar2Cartesian(roundedAngle, self.radius)
}
// returns position relative to the absolute parent, i.e Sun
Position getAbsolutePosition(int days) {
position = self.getRelativePosition(days)
if (self.parent) {
parentPosition = parent.getAbsolutePosition(d)
// Math.relativePosition converts position with respect to parents'
position = Math.relativePosition(position, parentPosition)
}
return position
}
}
class Sun extends Body{
// position will be (0,0)
}
class Planet extends OrbitingBody{
//parent will be Sun
}
class Moon extends OrbitingBody{
//parent will be a Planet
}
The above classes describe a basic modal of our solar system.
The method getAbsolutePosition finds position of the moon with respect to sun. For a planet this is straight forward.
For moon, there is a complication, as it returns the position relative to sun. getRelativePosition will always return relative to its parent, i.e for moon it will return w.r.t earth.
My objective here is to unit test the getAbsolutePosition.
If I have to test this for moon, I have to find out the return values that requires quite some manual effort, for a bunch of test cases. If there is any change in math logic, that would mean I have to find the updated return values again manually.
What are good software engineering practices to test functions like these?
You are correct in your assumption that tests which require "heavy computational work" in order to determine the "expected" values to check your actual results against would really suffer if base assumptions change and the expected results would change as result of that.
I see one option here: if possible, simply compute the expected test results, too, but don't code that yourself. You see, if you create test code and production code, chances are that you could make the same mistakes twice. So even when you would try to implement the required equations "in a different way" you doing test and production code isn't exactly ideal.
Thus: if possible, find somebody else who puts down computational logic to be used within your test cases. Maybe that code be somehow simplified - those "generators" don't need to be perfect; but they should give correct results for those corners you will be using them.
I often resolve these situations by writing implementation first, then using it to calculate values for test inputs, then I only verify them by hand, which is usually tiny bit easier than calculating them completely (although there's risk of introducing some bug into expected values from bug in code, I my verification fails).
Sometimes to get at least some distinction from the implementation code I do the math in Libre Office calc sheet, so I'm using the same formulas, but written second time by me, and into cells, so I usually end with a bit different structure and make sure one more time, that the formulas are as I wanted them.
Also try to decouple the complex calculations into several simpler steps, so you can mostly unit test the simple operations, then treat the whole operation more like integration test, testing only few cases. In case the internal logic will be adjusted, it will maybe break all those complex tests, but the simple operations tests will work for those which didn't change.
If this would be some real world astrology (it looks like 2D simplified space model, probably for some interactive animation), you would be maybe able to find some example data already computed by others, in some book/etc, so you could use those, that helps a lot (especially to find bugs in calculations of others :)).
And if you don't need super accurate results, but you just need to know that Moon was on Earth orbit and toward Sun, then you can test results against expected values guessed by hand with some big margin for error (like 1/4 of orbit distance, etc).
I'm pretty new to C++/Cocos2d, but I've been making pretty good progress. :)
What I want to do is animate a coin 'falling off' the screen after a player gets it. I've managed to successfully implement it 2 different ways, but each way has major downsides.
The goal: After a player gets a coin, the coin should 'jump', then 'fall' off of the screen. Ideally, the coin acts as if acted upon by gravity, so it jumps up with a fast speed, slows down to a stop, then proceeds to go downward at an increasing rate.
Attempts so far:
void Coin::tick(float dt) {
velocityY += gravity * dt;
float newX = coin->getPositionX() + velocityX;
float newY = coin->getPositionY() + velocityY;
coin->setPosition(newX, newY);
// using MoveBy(dt, Vec2(newX, newY)) has same result
}
// This is run on every 'update' of the main game loop.
This method does exactly what I would like it to do as far as movement, however, the frame rate gets extremely choppy and it starts to 'jump' between frames, sometimes quite significant distances.
ccBezierConfig bz;
bz.controlPoint_1 = Vec2(0, 0);
bz.controlPoint_2 = Vec2(20, 50); // These are just test values. Will normally be randomized to a degree.
bz.endPosition = Vec2(100, -2000);
auto coinDrop = BezierBy::create(2, bz);
coin->runAction(coinDrop);
This one has the benefit of 'perfect' framerate, where there is no choppiness whatsoever, however, it moves at a constant rate which ruins the experience of it falling and just makes it look like it's arbitrarily moving along some set path. (Which, well, it is.)
Has anybody run into a similar situation or know of a fix? Either to better handle the frame rate of the first one (MoveBy/To don't work- still has the choppy effect) or to programmatically set speeds of the second one (change speeds going to/from certain points in the curve)
Another idea I've had is to use a number of different MoveBy actions with different speeds, but that would have awkward 'pointy' curves and awkward changes in speed, so not really a solution.
Any ideas/help are/is greatly appreciated. :)
Yes, I have run into a similar situation. This is where 'easing' comes in handy. There are many built in easing functions that you can use such as Ease In or Ease Out. So your new code would look something like:
coin->runAction(cocos2d::EaseBounceOut::create(coinDrop));
This page shows the graphs for several common easing methods:
http://cocos2d-x.org/docs/programmers-guide/4/index.html
For your purposes (increasing speed over time) I would recommend trying the 'EaseIn' method.
I'm trying to simulate some real jump physics with setVelocity using Box2d in Cocos2d x C++.
My commands are like:
switch (keyCode){
case EventKeyboard::KeyCode::KEY_LEFT_ARROW:
physicsBody->setVelocity(Vec2(75, 0));
mySprite1->setPhysicsBody(physicsBody3);
break;
case EventKeyboard::KeyCode::KEY_RIGHT_ARROW:
physicsBody3->setVelocity(Vec2(-75,0));
mySprite1->setPhysicsBody(physicsBody3);
break;
case EventKeyboard::KeyCode::KEY_UP_ARROW:
physicsBody3->setVelocity(Vec2(0,200));
mySprite1->setPhysicsBody(physicsBody3);
break;
}
However, as it is clear to me, whenever i change the velocity in one axis, the other axis gets one.
The problem is: i'm unable to run and jump (the jump stops the running).
I need some way to change the velocity in only one axis at a time, so that my jump don't interfere with my run. That or another way to do tat exact same thing. I'm open to ideas regarding the physics since i'm a newbie in game programming.
You should use the ApplyLinearImpulse to get the effect that you are looking for.
// Apply linear impulse only in x-direction
physicsBody->ApplyLinearImpulse(b2Vec(75, 0), physicsBody->GetPosition(), true);
// Apply the jump impulse
physicsBody->ApplyLinearImpulse(b2Vec(0, 200), physicsBody->GetPosition(), true);
The above code will apply impulse and add to the current velocity. Which means that when you jump you will keep moving in the x-direction. This is code from one of my projects so I can confirm that it works.
I need to develop cannon game using box2d and cocos2dx. Cannon is divided into two parts
Base which is fixed in the ground and doesn't move.
b2WeldJointDef basePlatformWeldJointDef;
basePlatformWeldJointDef.Initialize(base->getBody(), weaponPlatform->getBody(),weaponPlatform->getBody()->GetWorldCenter());
basePlatformWeldJointDef.collideConnected = false;
basePlatformWeldJoint = m_world->CreateJoint(&basePlatformWeldJointDef);
Arm which is fixed in the base with a RevoluteJoint.
b2RevoluteJointDef armJointDef;
armJointDef.Initialize(base->getBody(), arm->getBody(), m_loader->pointsToMeters(ccp(armPosition.x-(arm->getContentSize().width*WeaponScale/4),armPosition.y)));
armJointDef.enableMotor = true;
armJointDef.enableLimit = true;
armJointDef.motorSpeed = 0;
armJointDef.lowerAngle = CC_DEGREES_TO_RADIANS(0);
armJointDef.upperAngle = CC_DEGREES_TO_RADIANS(0);
armJointDef.maxMotorTorque = 2;
armJointDef.collideConnected = false;
cannonRevoluteJoint = (b2RevoluteJoint*)m_world->CreateJoint(&armJointDef);
The cannon should calculate the angle that will fire to, and this is calculated correctly. Using the revolutJointObject->setLimits(lowerAngle, higherAngle) here i use the lowerAngle and the higherAngle both are the desired angle so the arm moves to desired angle directly without motion.
Then i needed to move the arm not just change its angle, so i set the lowerAngle and higherAngle with the old angle and desired angle depending which will be the lower and the which will the higher, and changed the motor speed of the RevoluteJoint
The problem is that i need a callback method which fires when the revolute joint reaches to a certain angle or when the motor speed reaches Zero assuming that when the arm reaches the limit angle will stop and the motor speed will be Zero.
I have searching in SO and i found This Solution which calculates the angle each step, this can be used but it needs more calculations that i feel lazy to do.
Does the callback method for notifying motor speed or certain angle has been reached exists ?
Schedule a function to check your conditions on each frame. When you don't need the checks any more - unschedule the selector, then reschedule when needed again.
As far as I know there are no callbacks for your exact task.
All a callback would do behind the scenes is check the angle each frame and then call the callback function when the joint angle is within a certain range.
There's no built in callbacks for this use case. You could make your own to hide what's going on, or you could just put the if statement in the update function. The performance of this will be fine.
I want to limit maximum speed a body can travel with.
The problem is, even if I do something like this answer suggests:
/* after applying forces from input for example */
b2Vec2 vel = body->GetLinearVelocity();
float speed = vel.Normalize();//normalizes vector and returns length
if ( speed > maxSpeed )
body->SetLinearVelocity( maxSpeed * vel );
What if, for example, right before clamping the velocity I am applying some huge force to the body?
Even if linear velocity is capped to maxSpeed for the moment, in the next timestep Box2D will take the b2Body::m_force value into account and effectively move my body faster than maxSpeed.
So I came up with this (had to move b2Body::m_force to public):
if ( speed > maxSpeed ) {
body->SetLinearVelocity( maxSpeed * vel );
body->m_force = b2Vec2(0, 0)
}
Yet this still doesn't handle the problem properly.
What if the velocity is slightly smaller than maxSpeed so the condition will not be hit, but still the m_force value will be big enough to increase velocity too much?
The point is I can't make accurate predictions as to how force will impact the velocity as I am stepping using delta accumulator and I don't know how many physics steps will be required for the moment.
Is there any way to handle this other than just to limit the velocity directly before integrating position in Box2D source code?
My first attempt to solve this problem was by simply executing above pieces of code not every loop, but every physics substep, which means that if my delta accumulator tells me I have to perform n b2World::Step's, I also cap the velocity n times:
// source code taken form above link and modified for my purposes
for (int i = 0; i < nStepsClamped; ++ i)
{
resetSmoothStates_ ();
// here I execute whole systems that apply accelerations, drag forces and limit maximum velocities
// ...
if ( speed > maxSpeed )
body->SetLinearVelocity( maxSpeed * vel );
// ...
singleStep_ (FIXED_TIMESTEP);
// NOTE I'M CLEARING FORCES EVERY SUBSTEP to avoid excessive accumulation
world_->ClearForces ();
}
Now while this gives me constant speed regardless of frame rate (which was my primary concern as my movement was jittery), it is not always <= maxSpeed. The same scenario: imagine a huge force applied just before capping the velocity and exceuting b2World::Step.
Now, I could simply calculate the actual force to be applied according to the current velocity, as I know the force will be applied only once until next validation, but there's another simple solution that I've already mentioned and eventually sticked with:
Go to Box2D\Dynamics\b2Body.h
Add float32 m_max_speed public member and initialize it with -1.f so initially we don't limit velocities for any body.
Go to Box2D\Dynamics\b2Island.cpp.
Locate line number 222.
Add following if condition:
m_positions[i].c = c;
m_positions[i].a = a;
if (b->m_max_speed >= 0.f) {
float32 speed = v.Normalize();
if (speed > b->m_max_speed)
v *= b->m_max_speed;
else v *= speed;
}
m_velocities[i].v = v;
m_velocities[i].w = w;
This will work even without substepping that I've described above but keep in mind that if you were to simulate air resistance, applying drag force every substep guarantees correctness of the simulation even with varying framerates.
First, answer for yourself, who can apply force to a body. Box2D itself can impact bodies via contacts and gravity. Contacts are not using forces, but impulses. To manage them setup contact listener and modify normalImpulses and tangentImpulses . Gravity I think cant impact body a lot, but it also can be controlled via b2BodyDef::gravityScale.
If your code aplying some manual forces, it maybe usefull to introduce some proxy interface to manage them.
I cant see some easy way, because at each step box2d makes several velocity and position iterations. So, forces and impulses applied to it at begin of step will cause change of position accordingly.
I cant imagine the way, how strict velocity without hacking box2d source code. By the way, I think it is not bad variant. For example, insert restriction in Dynamics/b2Island.cpp:219 (b2Island::Solve) to w and v variables.