sidefork.cc

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

/*
 *  Gazebo - Outdoor Multi-Robot Simulator
 *  Copyright (C) 2003  
 *     Nate Koenig & Andrew Howard
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/* Desc: Sidefork for  Shrimp model
 * Author: Stijn Opheide, Jef Marien, Koen Jans
 * Date: 8 May 2003
 * CVS: $Id: SideFork.cc,v 1.5 2005/08/10 15:09:28 natepak Exp $
 */

#include <assert.h>
#include "gazebo.h"
#include "World.hh"
#include "Body.hh"
#include "BoxGeom.hh"
#include "HingeJoint.hh"
#include "SideFork.hh"

ShrimpSideFork::ShrimpSideFork(Model *model){
	double barWidth = 0.018;
	double horizontalBarLength = 0.196;
	double verticalBarLength = 0.171;
	double barMass = 0.05;

	this->model = model;
	Geom *geom;

	//create the top bar
	topBar = new Body(model->world);
	geom = new BoxGeom(topBar, model->modelSpaceId,horizontalBarLength,barWidth,barWidth);
	geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
	geom->SetMass(barMass);
	geom->SetColor( GzColor(0, 1, 0) );

	//create the bottom bar
	bottomBar = new Body(model->world);
	geom = new BoxGeom(bottomBar, model->modelSpaceId,horizontalBarLength,barWidth,barWidth);
	geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
	geom->SetMass(barMass);
	geom->SetColor( GzColor(0, 1, 0) );

	//create the Front bar
	frontBar = new Body(model->world);
	geom = new BoxGeom(frontBar, model->modelSpaceId,verticalBarLength,barWidth,barWidth);
	geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
	geom->SetMass(barMass);
	geom->SetColor( GzColor(0, 1, 0) );

	//create the Back bar
	backBar = new Body(model->world);
	geom = new BoxGeom(backBar, model->modelSpaceId,verticalBarLength,barWidth,barWidth);
	geom->SetRelativePosition( GzVectorSet(0, 0, 0) );
	geom->SetMass(barMass);
	geom->SetColor( GzColor(0, 1, 0) );

	//position the bars
	topBar->SetPosition(GzVectorSet(0,0,-barWidth/2));
	bottomBar->SetPosition(GzVectorSet(0,0,-0.068 + barWidth/2));
	frontBar->SetPosition(GzVectorSet(-horizontalBarLength/2,0,-verticalBarLength/2));
	backBar->SetPosition(GzVectorSet(horizontalBarLength/2,0,-verticalBarLength/2));
	frontBar->SetRotation(GzQuaternFromAxis(0, 1, 0, -M_PI / 2));
	backBar->SetRotation(GzQuaternFromAxis(0, 1, 0, -M_PI / 2));

	//create the joints

	topFrontJoint = new HingeJoint( model->world );
	topFrontJoint->Attach(topBar,frontBar);
	topFrontJoint->SetAnchor(GzVectorSet(-horizontalBarLength/2,0,-barWidth/2));
	topFrontJoint->SetAxis(GzVectorSet(0,1,0));

	topBackJoint = new HingeJoint( model->world );
	topBackJoint->Attach(topBar,backBar);
	topBackJoint->SetAnchor(GzVectorSet(horizontalBarLength/2,0,-barWidth/2));
	topBackJoint->SetAxis(GzVectorSet(0,1,0));

	bottomBackJoint = new HingeJoint( model->world );
	bottomBackJoint->Attach(bottomBar,backBar);
	bottomBackJoint->SetAnchor(GzVectorSet(horizontalBarLength/2,0,-0.068 + barWidth/2));
	bottomBackJoint->SetAxis(GzVectorSet(0,1,0));

	bottomFrontJoint = new HingeJoint( model->world );
	bottomFrontJoint->Attach(bottomBar,frontBar);
	bottomFrontJoint->SetAnchor(GzVectorSet(-horizontalBarLength/2,0,-0.068 + barWidth/2));
	bottomFrontJoint->SetAxis(GzVectorSet(0,1,0));
	return;
}


void ShrimpSideFork::Translate(double x,double y, double z){
	//reposition the bars;
	GzVector a;
	a = topBar->GetPosition();
	topBar->SetPosition(GzVectorSet(a.x + x, a.y + y, a.z + z));

	a = bottomBar->GetPosition();
	bottomBar->SetPosition(GzVectorSet(a.x + x, a.y + y, a.z + z));

	a = frontBar->GetPosition();
	frontBar->SetPosition(GzVectorSet(a.x + x, a.y + y, a.z + z));

	a = backBar->GetPosition();
	backBar->SetPosition(GzVectorSet(a.x + x, a.y + y, a.z + z));
}

ShrimpSideFork::~ShrimpSideFork(){
	return;
}

Body *ShrimpSideFork::GetTopBar(){
	return topBar;
}

Body *ShrimpSideFork::GetBottomBar(){
	return bottomBar;
}

Body *ShrimpSideFork::GetFrontBar(){
	return frontBar;
}

Body *ShrimpSideFork::GetBackBar(){
	return backBar;
}