Using btFixedConstraint to simulate two servos.

Post Reply
svedach
Posts: 12
Joined: Mon Sep 01, 2014 6:53 am

Using btFixedConstraint to simulate two servos.

Post by svedach »

Good afternoon. Faced with the problem of simulating servo motors, the controled position of the body in two axes. Controlled body should have an arbitrary angle with respect to the installation of the base body. To link the two bodies using btFixedConstraint enabled on two axes motors in servo mode. I receive the following: the management of servos controlled body moves against their coordinate system, ie, instead of what would turn on the Z axis, it starts to rotate on the axis X. Now try to solve the problem by introducing an additional body fixedly tied to the base at the required angle, and to him with a zero turning join controlled body and has run this compound.
The initial version of the code that would like to implement, but which does not work:

Code: Select all

	_IsExist		= false;
	if ((ElemA.PhysicalBody() == NULL)||(ElemB.PhysicalBody() == NULL))		return;
	if (Axis.x() > 0)	Axis.setX(1);	if (Axis.y() > 0)	Axis.setY(1);	if (Axis.z() > 0)	Axis.setZ(1);
	if ((Axis.x() + Axis.y() + Axis.z()) != 2)								return;
	_Axis				= Axis;
	btTransform			FrameA, FrameB;
	btQuaternion		Rot;
	FrameA.setIdentity();
	FrameB.setIdentity();
	FrameA.setOrigin(PosInA);
	FrameB.setOrigin(PosInB);
	Rot.setEuler(DEG_TO_RAD * RotInB.y(), DEG_TO_RAD * RotInB.z(), DEG_TO_RAD * RotInB.x());
	FrameA.setRotation(Rot);
	_Constraint		= new btFixedConstraint(*ElemA.PhysicalBody(), *ElemB.PhysicalBody(), FrameA, FrameB);
	ElemB.PhysicalBody()->setActivationState(DISABLE_DEACTIVATION);
	_Constraint->setLimit(0, 0, 0);
	_Constraint->setLimit(1, 0, 0);
	_Constraint->setLimit(2, 0, 0);
	_Constraint->setLimit(3, DEG_TO_RAD * -175 * _Axis.x(), DEG_TO_RAD * 175 * _Axis.x());
	_Constraint->setLimit(4, DEG_TO_RAD * -85  * _Axis.y(), DEG_TO_RAD * 85  * _Axis.y());
	_Constraint->setLimit(5, DEG_TO_RAD * -175 * _Axis.z(), DEG_TO_RAD * 175 * _Axis.z());
	_Constraint->enableMotor(3, _Axis.x());
	_Constraint->enableMotor(4, _Axis.y());
	_Constraint->enableMotor(5, _Axis.z());
	if (_Axis == btVector3(1, 1, 0))
	{
		_Mult0	= DEG_TO_RAD * btScalar(350) / btScalar(255);
		_Mult1	= DEG_TO_RAD * btScalar(170) / btScalar(255);
	}
	else if (_Axis == btVector3(0, 1, 1))
		{
			_Mult0	= DEG_TO_RAD * btScalar(170) / btScalar(255);
			_Mult1	= DEG_TO_RAD * btScalar(350) / btScalar(255);
		}
		else if (_Axis == btVector3(1, 0, 1))
			{
				_Mult0	= DEG_TO_RAD * btScalar(350) / btScalar(255);
				_Mult1	= DEG_TO_RAD * btScalar(170) / btScalar(255);
			}
			else
			{
				_Mult0	= 0;
				_Mult1	= 0;
			}
	_Constraint->setServo(3, true);
	_Constraint->setServo(4, true);
	_Constraint->setServo(5, true);
	
	_Speed		= 1;
	_Effort		= 1000;
	_Constraint->setTargetVelocity(3, _Speed);
	_Constraint->setTargetVelocity(4, _Speed);
	_Constraint->setTargetVelocity(5, _Speed);
	_Constraint->setMaxMotorForce(3, _Effort);
	_Constraint->setMaxMotorForce(4, _Effort);
	_Constraint->setMaxMotorForce(5, _Effort);

	g_PhysicalWorld->World()->addConstraint(_Constraint, true);
	_IsExist		= true;
Sorry for bad english.
Post Reply