コード例 #1
0
    virtual void	stepSimulation(float deltaTime)
	{
        if ((m_options & eGRIPPER_GRASP)!=0)
        {
            if ((m_gripperIndex>=0))
            {
                int fingerJointIndices[3]={0,1,3};
                double fingerTargetVelocities[3]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
                    ,-sGripperClosingTargetVelocity
                };
                double maxTorqueValues[3]={40.0,50.0,50.0};
                for (int i=0;i<3;i++)
                {
                    b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
                    controlArgs.m_targetVelocity = fingerTargetVelocities[i];
                    controlArgs.m_maxTorqueValue = maxTorqueValues[i];
                    controlArgs.m_kd = 1.;
                    m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
                }
            }
        }
        
        if ((m_options & eONE_MOTOR_GRASP)!=0)
        {
            int fingerJointIndices[2]={0,1};
            double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
            };
            double maxTorqueValues[2]={800.0,800.0};
            for (int i=0;i<2;i++)
            {
                b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
                controlArgs.m_targetVelocity = fingerTargetVelocities[i];
                controlArgs.m_maxTorqueValue = maxTorqueValues[i];
                controlArgs.m_kd = 1.;
                m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
            }
        }
        
        if ((m_options & eGRASP_SOFT_BODY)!=0)
        {
            int fingerJointIndices[2]={0,1};
            double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
            };
            double maxTorqueValues[2]={50.0,10.0};
            for (int i=0;i<2;i++)
            {
                b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
                controlArgs.m_targetVelocity = fingerTargetVelocities[i];
                controlArgs.m_maxTorqueValue = maxTorqueValues[i];
                controlArgs.m_kd = 1.;
                m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
            }
        }
        
		m_robotSim.stepSimulation();
    }
コード例 #2
0
void setJointMotorPositionControl(b3RobotSimulatorClientAPI* sim, int obUid, int linkIndex, double targetPosition)
{
	b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
	controlArgs.m_maxTorqueValue = 50.;
	controlArgs.m_targetPosition = targetPosition;
	controlArgs.m_targetVelocity = 0;
	sim->setJointMotorControl(obUid, linkIndex, controlArgs);
}
コード例 #3
0
int main(int argc, char* argv[])
{
	b3CommandLineArgs args(argc, argv);
	std::string port = "COM9";
	args.GetCmdLineArgument("port", port);
	int baud = 115200;
	args.GetCmdLineArgument("speed", baud);
	std::string mode = "SHARED_MEMORY";
	args.GetCmdLineArgument("mode", mode);
	int disableGui = 0;
	args.GetCmdLineArgument("disableGui", disableGui);
	int disableShadows = 0;
	args.GetCmdLineArgument("disableShadows", disableShadows);
	int useKitchen = 0;
	args.GetCmdLineArgument("useKitchen", useKitchen);

	int deviceTypeFilter = VR_DEVICE_GENERIC_TRACKER;
	args.GetCmdLineArgument("deviceTypeFilter", deviceTypeFilter);

	printf("port=%s, speed=%d, connection mode=%s\n", port.c_str(), baud, mode.c_str());

	b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI();

	//Can also use eCONNECT_UDP,eCONNECT_TCP, for example: sim->connect(eCONNECT_UDP, "localhost", 1234);
	if (mode == "GUI")
	{
		sim->connect(eCONNECT_GUI);
	}
	else
	{
		if (mode == "DIRECT")
		{
			sim->connect(eCONNECT_DIRECT);
		}
		else
		{
			sim->connect(eCONNECT_SHARED_MEMORY);
		}
	}

	sim->setRealTimeSimulation(true);
	sim->setInternalSimFlags(0);
	sim->resetSimulation();

	if (disableGui)
	{
		sim->configureDebugVisualizer(COV_ENABLE_GUI, 0);
	}

	if (disableShadows)
	{
		sim->configureDebugVisualizer(COV_ENABLE_SHADOWS, 0);
	}

	sim->setTimeOut(12345);
	//syncBodies is only needed when connecting to an existing physics server that has already some bodies
	sim->syncBodies();
	b3Scalar fixedTimeStep = 1. / 240.;

	sim->setTimeStep(fixedTimeStep);

	b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1, 0.2, 0.3));
	b3Vector3 rpy;
	rpy = sim->getEulerFromQuaternion(q);

	sim->setGravity(b3MakeVector3(0, 0, -9.8));
	sim->setContactBreakingThreshold(0.001);

	if (useKitchen)
	{
		b3RobotSimulatorLoadFileResults res;
		sim->loadSDF("kitchens/1.sdf", res);
	}
	else
	{
		sim->loadURDF("plane_with_collision_audio.urdf");
	}

	int handUid = -1;

	b3RobotSimulatorLoadFileResults mjcfResults;
	const char* mjcfFileName = "MPL/mpl2.xml";
	if (sim->loadMJCF(mjcfFileName, mjcfResults))
	{
		printf("mjcfResults = %d\n", mjcfResults.m_uniqueObjectIds.size());
		if (mjcfResults.m_uniqueObjectIds.size() == 1)
		{
			handUid = mjcfResults.m_uniqueObjectIds[0];
		}
	}
	if (handUid < 0)
	{
		printf("Cannot load MJCF file %s\n", mjcfFileName);
	}

#ifdef TOUCH
	b3Vector3 handPos = b3MakeVector3(-0.10, -0.03, 0.02);
	b3Vector3 rollPitchYaw = b3MakeVector3(0.5 * B3_PI, 0, 1.25 * B3_PI);  //-B3_PI/2,0,B3_PI/2);
	handOrn = sim->getQuaternionFromEuler(rollPitchYaw);

#else
	b3Quaternion handOrn = sim->getQuaternionFromEuler(b3MakeVector3(3.14, -3.14 / 2, 0));
	b3Vector3 handPos = b3MakeVector3(-0.05, 0, 0.02);
#endif

	b3Vector3 handStartPosWorld = b3MakeVector3(0.500000, 0.300006, 0.900000);
	b3Quaternion handStartOrnWorld = b3Quaternion ::getIdentity();

	b3JointInfo jointInfo;
	jointInfo.m_jointType = eFixedType;

	printf("handStartOrnWorld=%f,%f,%f,%f\n", handStartOrnWorld[0], handStartOrnWorld[1], handStartOrnWorld[2], handStartOrnWorld[3]);
	jointInfo.m_childFrame[0] = handStartPosWorld[0];
	jointInfo.m_childFrame[1] = handStartPosWorld[1];
	jointInfo.m_childFrame[2] = handStartPosWorld[2];
	jointInfo.m_childFrame[3] = handStartOrnWorld[0];
	jointInfo.m_childFrame[4] = handStartOrnWorld[1];
	jointInfo.m_childFrame[5] = handStartOrnWorld[2];
	jointInfo.m_childFrame[6] = handStartOrnWorld[3];

	jointInfo.m_parentFrame[0] = handPos[0];
	jointInfo.m_parentFrame[1] = handPos[1];
	jointInfo.m_parentFrame[2] = handPos[2];
	jointInfo.m_parentFrame[3] = handOrn[0];
	jointInfo.m_parentFrame[4] = handOrn[1];
	jointInfo.m_parentFrame[5] = handOrn[2];
	jointInfo.m_parentFrame[6] = handOrn[3];

	sim->resetBasePositionAndOrientation(handUid, handStartPosWorld, handStartOrnWorld);
	int handConstraintId = sim->createConstraint(handUid, -1, -1, -1, &jointInfo);
	double maxFingerForce = 10;
	double maxArmForce = 1000;
	for (int j = 0; j < sim->getNumJoints(handUid); j++)
	{
		b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
		controlArgs.m_maxTorqueValue = maxFingerForce;
		controlArgs.m_kp = 0.1;
		controlArgs.m_kd = 1;
		controlArgs.m_targetPosition = 0;
		controlArgs.m_targetVelocity = 0;
		sim->setJointMotorControl(handUid, j, controlArgs);
	}

	sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.300000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
	sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.200000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
	sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.100000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
	sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
	sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.900000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
	sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.800000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
	sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.700000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
	sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.600000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107)));
	sim->loadURDF("table/table.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000, -0.200000, 0.000000), b3Quaternion(0.000000, 0.000000, 0.707107, 0.707107)));
	sim->loadURDF("cube_small.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.950000, -0.100000, 0.700000), b3Quaternion(0.000000, 0.000000, 0.707107, 0.707107)));
	sim->loadURDF("sphere_small.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.850000, -0.400000, 0.700000), b3Quaternion(0.000000, 0.000000, 0.707107, 0.707107)));

	b3Clock clock;
	double startTime = clock.getTimeInSeconds();
	double simWallClockSeconds = 20.;

	int vidLogId = -1;
	int minitaurLogId = -1;
	int rotateCamera = 0;
	serial::Serial my_serial;

	try
	{
		// port, baudrate, timeout in milliseconds
		my_serial.setBaudrate(baud);
		my_serial.setPort(port);
		my_serial.setBytesize(serial::sevenbits);
		my_serial.setParity(serial::parity_odd);
		my_serial.setStopbits(serial::stopbits_two);
		my_serial.setTimeout(serial::Timeout::simpleTimeout(0.01));
		my_serial.open();
	}
	catch (...)
	{
		printf("Cannot open port, use --port=PORTNAME\n");
		exit(0);
	}

	if (!my_serial.isOpen())
	{
		printf("Cannot open serial port\n");
		return -1;
	}

	my_serial.flush();

	while (sim->canSubmitCommand())
	{
		clock.usleep(1);

		b3VREventsData vrEvents;

		sim->getVREvents(&vrEvents, deviceTypeFilter);
		//instead of iterating over all vr events, we just take the most up-to-date one
		if (vrEvents.m_numControllerEvents)
		{
			int i = vrEvents.m_numControllerEvents - 1;
			b3VRControllerEvent& e = vrEvents.m_controllerEvents[i];
			//			printf("e.pos=%f,%f,%f\n",e.m_pos[0],e.m_pos[1],e.m_pos[2]);
			b3JointInfo changeConstraintInfo;
			changeConstraintInfo.m_flags = 0;
			changeConstraintInfo.m_jointMaxForce = maxArmForce;
			changeConstraintInfo.m_flags |= eJointChangeMaxForce;

			changeConstraintInfo.m_childFrame[0] = e.m_pos[0];
			changeConstraintInfo.m_childFrame[1] = e.m_pos[1];
			changeConstraintInfo.m_childFrame[2] = e.m_pos[2];
			changeConstraintInfo.m_flags |= eJointChangeChildFramePosition;

			changeConstraintInfo.m_childFrame[3] = e.m_orn[0];
			changeConstraintInfo.m_childFrame[4] = e.m_orn[1];
			changeConstraintInfo.m_childFrame[5] = e.m_orn[2];
			changeConstraintInfo.m_childFrame[6] = e.m_orn[3];
			changeConstraintInfo.m_flags |= eJointChangeChildFrameOrientation;

			sim->changeConstraint(handConstraintId, &changeConstraintInfo);
		}

		//read the serial output from the hand, extract into parts
		std::string result;
		try
		{
			result = my_serial.readline();
		}
		catch (...)
		{
		}
		if (result.length())
		{
			my_serial.flush();
			int res = result.find("\n");
			while (res < 0)
			{
				result += my_serial.readline();
				res = result.find("\n");
			}
			btAlignedObjectArray<std::string> pieces;
			btAlignedObjectArray<std::string> separators;
			separators.push_back(",");
			urdfStringSplit(pieces, result, separators);

			//printf("serial: %s\n", result.c_str());
			if (pieces.size() == 6)
			{
				double pinkTarget = 0;
				double middleTarget = 0;
				double indexTarget = 0;
				double thumbTarget = 0;
				{
					int pink = atoi(pieces[1].c_str());
					int middle = atoi(pieces[2].c_str());
					int index = atoi(pieces[3].c_str());
					int thumb = atoi(pieces[4].c_str());

					pinkTarget = convertSensor(pink, 250, 400);
					middleTarget = convertSensor(middle, 250, 400);
					indexTarget = convertSensor(index, 250, 400);
					thumbTarget = convertSensor(thumb, 250, 400);
				}

				//printf("pink = %d, middle=%d, index=%d, thumb=%d\n", pink,middle,index,thumb);

				setJointMotorPositionControl(sim, handUid, 5, 1.3);
				setJointMotorPositionControl(sim, handUid, 7, thumbTarget);
				setJointMotorPositionControl(sim, handUid, 9, thumbTarget);
				setJointMotorPositionControl(sim, handUid, 11, thumbTarget);

				setJointMotorPositionControl(sim, handUid, 15, indexTarget);
				setJointMotorPositionControl(sim, handUid, 17, indexTarget);
				setJointMotorPositionControl(sim, handUid, 19, indexTarget);

				setJointMotorPositionControl(sim, handUid, 22, middleTarget);
				setJointMotorPositionControl(sim, handUid, 24, middleTarget);
				setJointMotorPositionControl(sim, handUid, 27, middleTarget);

				double ringTarget = 0.5f * (pinkTarget + middleTarget);
				setJointMotorPositionControl(sim, handUid, 30, ringTarget);
				setJointMotorPositionControl(sim, handUid, 32, ringTarget);
				setJointMotorPositionControl(sim, handUid, 34, ringTarget);

				setJointMotorPositionControl(sim, handUid, 38, pinkTarget);
				setJointMotorPositionControl(sim, handUid, 40, pinkTarget);
				setJointMotorPositionControl(sim, handUid, 42, pinkTarget);
			}
		}

		b3KeyboardEventsData keyEvents;
		sim->getKeyboardEvents(&keyEvents);

		//sim->stepSimulation();

		if (rotateCamera)
		{
			static double yaw = 0;
			double distance = 1;
			yaw += 0.1;
			b3Vector3 basePos;
			b3Quaternion baseOrn;
			//			sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn);
			//			sim->resetDebugVisualizerCamera(distance,yaw,20,basePos);
		}
		//b3Clock::usleep(1000.*1000.*fixedTimeStep);
	}

	printf("serial.close()\n");
	my_serial.close();

	printf("sim->disconnect\n");
	sim->disconnect();

	printf("delete sim\n");
	delete sim;

	printf("exit\n");
}
コード例 #4
0
    virtual void    initPhysics()
    {

		int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
		m_robotSim.setGuiHelper(m_guiHelper);
		bool connected = m_robotSim.connect(mode);

		b3Printf("robotSim connected = %d",connected);
	
        if ((m_options & eGRIPPER_GRASP)!=0)
        {
            
            {
                SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
                slider.m_minVal=-2;
                slider.m_maxVal=2;
                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
            }
            
            {
                SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity
                                    );
                slider.m_minVal=-1;
                slider.m_maxVal=1;
                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
            }
			{

				b3RobotSimulatorLoadUrdfFileArgs args;
				args.m_startPosition.setValue(0, 0, .107);
				args.m_startOrientation.setEulerZYX(0, 0, 0);
				args.m_useMultiBody = true;
				m_robotSim.loadURDF("cube_small.urdf", args);
			}

            {
                b3RobotSimulatorLoadFileResults results;
                m_robotSim.loadSDF("gripper/wsg50_with_r2d2_gripper.sdf",results);
                if (results.m_uniqueObjectIds.size()==1)
                {
                    
                    m_gripperIndex = results.m_uniqueObjectIds[0];
                    int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
                    b3Printf("numJoints = %d",numJoints);
                    
                    for (int i=0;i<numJoints;i++)
                    {
                        b3JointInfo jointInfo;
                        m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo);
                        b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
                    }
                    
                    /*
                    int fingerJointIndices[2]={1,3};
                    double fingerTargetPositions[2]={-0.04,0.04};
                    for (int i=0;i<2;i++)
                    {
                        b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
                        controlArgs.m_targetPosition = fingerTargetPositions[i];
                        controlArgs.m_kp = 5.0;
                        controlArgs.m_kd = 3.0;
                        controlArgs.m_maxTorqueValue = 1.0;
                        m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
                    }
                    */
                    int fingerJointIndices[3]={0,1,3};
                    double fingerTargetVelocities[3]={-0.2,.5,-.5};
                    double maxTorqueValues[3]={40.0,50.0,50.0};
                    for (int i=0;i<3;i++)
                    {
                        b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
                        controlArgs.m_targetVelocity = fingerTargetVelocities[i];
                        controlArgs.m_maxTorqueValue = maxTorqueValues[i];
                        controlArgs.m_kd = 1.;
                        m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
                    }
                }
            }
           
            if (1)
            {
                m_robotSim.loadURDF("plane.urdf");
            }
            m_robotSim.setGravity(b3MakeVector3(0,0,-10));
            m_robotSim.setNumSimulationSubSteps(4);
        }

        if ((m_options & eTWO_POINT_GRASP)!=0)
        {
            {
                b3RobotSimulatorLoadUrdfFileArgs args;
                args.m_startPosition.setValue(0, 0, .107);
                m_robotSim.loadURDF("cube_small.urdf", args);
            }
            
            {
                b3RobotSimulatorLoadUrdfFileArgs args;
                args.m_startPosition.setValue(0.068, 0.02, 0.11);
                m_robotSim.loadURDF("cube_gripper_left.urdf", args);
                
                b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
                controlArgs.m_targetVelocity = -0.1;
                controlArgs.m_maxTorqueValue = 10.0;
                controlArgs.m_kd = 1.;
                m_robotSim.setJointMotorControl(1,0,controlArgs);
            }
            
            {
                b3RobotSimulatorLoadUrdfFileArgs args;
                args.m_startPosition.setValue(-0.068, 0.02, 0.11);
                m_robotSim.loadURDF("cube_gripper_right.urdf", args);
                
                b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
                controlArgs.m_targetVelocity = 0.1;
                controlArgs.m_maxTorqueValue = 10.0;
                controlArgs.m_kd = 1.;
                m_robotSim.setJointMotorControl(2,0,controlArgs);
            }
            
            if (1)
            {
                m_robotSim.loadURDF("plane.urdf");
                
            }
            m_robotSim.setGravity(b3MakeVector3(0,0,-10));
            m_robotSim.setNumSimulationSubSteps(4);
        }
        
        if ((m_options & eONE_MOTOR_GRASP)!=0)
        {
	    m_robotSim.setNumSolverIterations(150);
            {
                SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
                slider.m_minVal=-2;
                slider.m_maxVal=2;
                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
            }
            
            {
                SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity
                                    );
                slider.m_minVal=-1;
                slider.m_maxVal=1;
                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
            }
            {
                b3RobotSimulatorLoadUrdfFileArgs args;
                args.m_startPosition.setValue(0, -0.2, .47);
                args.m_startOrientation.setEulerZYX(SIMD_HALF_PI, 0, 0);
                m_robotSim.loadURDF("dinnerware/pan_tefal.urdf", args);
            }
            {
                b3RobotSimulatorLoadFileResults args;
                b3RobotSimulatorLoadFileResults results;
                m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf",results);

                if (results.m_uniqueObjectIds.size()==1)
                {
                    
                    m_gripperIndex = results.m_uniqueObjectIds[0];
                    int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
                    b3Printf("numJoints = %d",numJoints);
                    
                    for (int i=0;i<numJoints;i++)
                    {
                        b3JointInfo jointInfo;
                        m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo);
                        b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
                    }
                    
                    for (int i=0;i<8;i++)
                    {
                        b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
                        controlArgs.m_maxTorqueValue = 0.0;
                        m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs);
                    }

                }
            }
            
            if (1)
            {
                m_robotSim.loadURDF("plane.urdf");
            }
            m_robotSim.setGravity(b3MakeVector3(0,0,-10));
            
            b3JointInfo revoluteJoint1;
            revoluteJoint1.m_parentFrame[0] = -0.055;
            revoluteJoint1.m_parentFrame[1] = 0;
            revoluteJoint1.m_parentFrame[2] = 0.02;
            revoluteJoint1.m_parentFrame[3] = 0;
            revoluteJoint1.m_parentFrame[4] = 0;
            revoluteJoint1.m_parentFrame[5] = 0;
            revoluteJoint1.m_parentFrame[6] = 1.0;
            revoluteJoint1.m_childFrame[0] = 0;
            revoluteJoint1.m_childFrame[1] = 0;
            revoluteJoint1.m_childFrame[2] = 0;
            revoluteJoint1.m_childFrame[3] = 0;
            revoluteJoint1.m_childFrame[4] = 0;
            revoluteJoint1.m_childFrame[5] = 0;
            revoluteJoint1.m_childFrame[6] = 1.0;
            revoluteJoint1.m_jointAxis[0] = 1.0;
            revoluteJoint1.m_jointAxis[1] = 0.0;
            revoluteJoint1.m_jointAxis[2] = 0.0;
            revoluteJoint1.m_jointType = ePoint2PointType;
            b3JointInfo revoluteJoint2;
            revoluteJoint2.m_parentFrame[0] = 0.055;
            revoluteJoint2.m_parentFrame[1] = 0;
            revoluteJoint2.m_parentFrame[2] = 0.02;
            revoluteJoint2.m_parentFrame[3] = 0;
            revoluteJoint2.m_parentFrame[4] = 0;
            revoluteJoint2.m_parentFrame[5] = 0;
            revoluteJoint2.m_parentFrame[6] = 1.0;
            revoluteJoint2.m_childFrame[0] = 0;
            revoluteJoint2.m_childFrame[1] = 0;
            revoluteJoint2.m_childFrame[2] = 0;
            revoluteJoint2.m_childFrame[3] = 0;
            revoluteJoint2.m_childFrame[4] = 0;
            revoluteJoint2.m_childFrame[5] = 0;
            revoluteJoint2.m_childFrame[6] = 1.0;
            revoluteJoint2.m_jointAxis[0] = 1.0;
            revoluteJoint2.m_jointAxis[1] = 0.0;
            revoluteJoint2.m_jointAxis[2] = 0.0;
            revoluteJoint2.m_jointType = ePoint2PointType;
            m_robotSim.createConstraint(1, 2, 1, 4, &revoluteJoint1);
            m_robotSim.createConstraint(1, 3, 1, 6, &revoluteJoint2);
        }
		
        if ((m_options & eGRASP_SOFT_BODY)!=0)
        {
            {
                SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
                slider.m_minVal=-2;
                slider.m_maxVal=2;
                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
            }
            
            {
                SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity
                                    );
                slider.m_minVal=-1;
                slider.m_maxVal=1;
                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
            }
            {
                b3RobotSimulatorLoadFileResults results;
				m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf",results);

                if (results.m_uniqueObjectIds.size()==1)
                {
                    
                    m_gripperIndex = results.m_uniqueObjectIds[0];
                    int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
                    b3Printf("numJoints = %d",numJoints);
                    
                    for (int i=0;i<numJoints;i++)
                    {
                        b3JointInfo jointInfo;
                        m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo);
                        b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
                    }
                    
                    for (int i=0;i<8;i++)
                    {
                        b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
                        controlArgs.m_maxTorqueValue = 0.0;
                        m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs);
                    }
                    
                }
            }
            {
				b3RobotSimulatorLoadUrdfFileArgs args;
                args.m_startPosition.setValue(0,0,-0.2);
                args.m_startOrientation.setEulerZYX(0,0,0);
                m_robotSim.loadURDF("plane.urdf", args);
                
            }
            m_robotSim.setGravity(b3MakeVector3(0,0,-10));
            m_robotSim.loadBunny(0.1,0.1,0.02);
            
            b3JointInfo revoluteJoint1;
            revoluteJoint1.m_parentFrame[0] = -0.055;
            revoluteJoint1.m_parentFrame[1] = 0;
            revoluteJoint1.m_parentFrame[2] = 0.02;
            revoluteJoint1.m_parentFrame[3] = 0;
            revoluteJoint1.m_parentFrame[4] = 0;
            revoluteJoint1.m_parentFrame[5] = 0;
            revoluteJoint1.m_parentFrame[6] = 1.0;
            revoluteJoint1.m_childFrame[0] = 0;
            revoluteJoint1.m_childFrame[1] = 0;
            revoluteJoint1.m_childFrame[2] = 0;
            revoluteJoint1.m_childFrame[3] = 0;
            revoluteJoint1.m_childFrame[4] = 0;
            revoluteJoint1.m_childFrame[5] = 0;
            revoluteJoint1.m_childFrame[6] = 1.0;
            revoluteJoint1.m_jointAxis[0] = 1.0;
            revoluteJoint1.m_jointAxis[1] = 0.0;
            revoluteJoint1.m_jointAxis[2] = 0.0;
            revoluteJoint1.m_jointType = ePoint2PointType;
            b3JointInfo revoluteJoint2;
            revoluteJoint2.m_parentFrame[0] = 0.055;
            revoluteJoint2.m_parentFrame[1] = 0;
            revoluteJoint2.m_parentFrame[2] = 0.02;
            revoluteJoint2.m_parentFrame[3] = 0;
            revoluteJoint2.m_parentFrame[4] = 0;
            revoluteJoint2.m_parentFrame[5] = 0;
            revoluteJoint2.m_parentFrame[6] = 1.0;
            revoluteJoint2.m_childFrame[0] = 0;
            revoluteJoint2.m_childFrame[1] = 0;
            revoluteJoint2.m_childFrame[2] = 0;
            revoluteJoint2.m_childFrame[3] = 0;
            revoluteJoint2.m_childFrame[4] = 0;
            revoluteJoint2.m_childFrame[5] = 0;
            revoluteJoint2.m_childFrame[6] = 1.0;
            revoluteJoint2.m_jointAxis[0] = 1.0;
            revoluteJoint2.m_jointAxis[1] = 0.0;
            revoluteJoint2.m_jointAxis[2] = 0.0;
            revoluteJoint2.m_jointType = ePoint2PointType;
            m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
            m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
        }
        
        if ((m_options & eSOFTBODY_MULTIBODY_COUPLING)!=0)
        {
            {
                b3RobotSimulatorLoadUrdfFileArgs args;
                args.m_startPosition.setValue(0,1.0,2.0);
                args.m_startOrientation.setEulerZYX(0,0,1.57);
                args.m_forceOverrideFixedBase = false;
                args.m_useMultiBody = true;
                int kukaId = m_robotSim.loadURDF("kuka_iiwa/model_free_base.urdf", args);
                
                int numJoints = m_robotSim.getNumJoints(kukaId);
                b3Printf("numJoints = %d",numJoints);
                
                for (int i=0;i<numJoints;i++)
                {
                    b3JointInfo jointInfo;
                    m_robotSim.getJointInfo(kukaId,i,&jointInfo);
                    b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
                    b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
                    controlArgs.m_maxTorqueValue = 0.0;
                    m_robotSim.setJointMotorControl(kukaId,i,controlArgs);
                }
            }
            {
                b3RobotSimulatorLoadUrdfFileArgs args;
                args.m_startPosition.setValue(0,0,0);
                args.m_startOrientation.setEulerZYX(0,0,0);
                args.m_forceOverrideFixedBase = true;
                args.m_useMultiBody = false;
                m_robotSim.loadURDF("plane.urdf", args);
            }
            m_robotSim.setGravity(b3MakeVector3(0,0,-10));
            m_robotSim.loadBunny(0.5,10.0,0.1);
        }
    }
コード例 #5
0
ファイル: archivefs.cpp プロジェクト: samekmichal/archivefs
bool initialize(FusePrivate* data) {
  int ret;
  ArchiveType* archive_type = NULL;

  if (data->drivers_path)
    path_to_drivers = realpath(data->drivers_path, NULL);

  LOAD_STANDARD_DRIVERS();
  if (drivers->empty()) return false;

  FileSystem::setBufferLimit(data->buffer_limit);
  if (data->keep_trash)     FileSystem::keep_trash = true;
  if (data->respect_rights) ArchiveDriver::respect_rights = true;
  if (data->keep_original)  ArchiveDriver::keep_original = true;

  /* První část inicializace */
  if (data->create_archive) {
    data->mode = FusePrivate::ARCHIVE_MOUNTED;

    char* path = (char*)malloc(PATH_MAX);
    memset(path, 0, PATH_MAX);

    getcwd(path, PATH_MAX);
    strcat(path, "/");
    strcat(path, data->mounted);
    data->mounted = path;

    /* soubor již existuje */
    if (access(path, F_OK) == 0) {
      cerr << "Archive file with specified name already exist" << endl;
      return false;
    }

    /* Najdeme koncovku a ovladač */
    const char* ext = findFileExt(data->mounted, NULL);
    archive_type = TYPE_BY_EXT(ext);
    if (archive_type == NULL) {
      cerr << "Error: this type of archive files is not supported" << endl;
      /* Při destrukci FusePrivate bude data->mounted uvolňován */
      data->mounted = NULL;
      return false;
    }

    if (!archive_type->write_support) {
      /* Při destrukci FusePrivate bude data->mounted uvolňován */
      data->mounted = NULL;
      cerr << "Write support for this type of archive is not implemented, sorry" << endl;
      return false;
    }
  } else {
    data->mounted = realpath(data->mounted, NULL);
    ret = errno;
    if (data->mounted == NULL) {
      cerr << "Error: " << strerror(ret) << endl;
      return false;
    }

    ret = setMountMode(data);
    if (ret == -1) {
      cerr << "Unsupported type of source file, sorry" << endl;
      return false;
    } else if (ret > 0) {
      cerr << "Error: " << strerror(errno) << endl;
      return false;
    }
  }

  /* Kontrola kombinace parametrů */
  if (!controlArgs(data)) {
    cerr << "Invalid combination of parameters" << endl;
    return false;
  }

  /* Druhá část inicializace - budování FileSystémů ze zdrojů */
  FileSystem* fs = NULL;
  if (data->mode == FusePrivate::ARCHIVE_MOUNTED) {
    /* Pokud se vytváří nový archiv, ovladač je již načten.
     * Pokud ne, je třeba jej načíst.
     */
    if (!data->create_archive) {
      /* data->mounted už je realpath-ed */
      archive_type = GET_TYPE(data->mounted);
      if (archive_type == NULL) {
        cerr << "Error: Could not load driver for " << data->mounted << endl;
        return false;
      }
    }

    try {
      fs = new FileSystem(data->mounted, data->create_archive, archive_type);
    }
    catch (ArchiveDriver::ArchiveError&) {
      cerr << "Error: Failed to open/process archive file" << endl;
      return false;
    }

    data->filesystems->insert(fs);

  } else {
    DIR* dir;
    struct dirent* file;
    struct stat info;

    char filename[PATH_MAX];
    char* filename_ptr;
    unsigned mounted_len = strlen(data->mounted);
    strcpy(filename, data->mounted);
    filename[mounted_len] = '/';
    filename_ptr = filename + mounted_len + 1;

    /* Projdu celý připojený adresář a pokud obsahuje archivy podporovaných typů
     * vytvořím jejich filesystémy */
    dir = opendir(data->mounted);
    while ((file = readdir(dir)) != NULL) {
      if (strcmp(file->d_name, ".") == 0 || strcmp(file->d_name, "..") == 0)
        continue;

      archive_type = NULL;

      // Získání celé cesty k archivu
      strcpy(filename_ptr, file->d_name);

      if (stat(filename, &info) != 0) continue;

      if (S_ISREG(info.st_mode) || S_ISLNK(info.st_mode)) {
        archive_type = GET_TYPE(filename);
        if (archive_type != NULL) {
          try {
            fs = new FileSystem(filename, false, archive_type);
          }
          catch (...) {
            continue;
          }

          data->filesystems->insert(fs);
        }
      }
    }

    closedir(dir);
  }

//   cout << "FILESYSTEM IS INITIALIZED" << endl;

  return true;
}
コード例 #6
0
    virtual void    initPhysics()
    {
		int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
		m_robotSim.setGuiHelper(m_guiHelper);
		bool connected = m_robotSim.connect(mode);

		b3Printf("robotSim connected = %d",connected);
		
		
		if ((m_options & eROBOTIC_LEARN_GRASP)!=0)
		{
			{
				b3RobotSimulatorLoadUrdfFileArgs args;
				args.m_startPosition.setValue(0,0,.5);
				m_r2d2Index = m_robotSim.loadURDF("r2d2.urdf",args);

				if (m_r2d2Index>=0)
				{
					int numJoints = m_robotSim.getNumJoints(m_r2d2Index);
					b3Printf("numJoints = %d",numJoints);

					for (int i=0;i<numJoints;i++)
					{
						b3JointInfo jointInfo;
						m_robotSim.getJointInfo(m_r2d2Index,i,&jointInfo);
						b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
					}
					int wheelJointIndices[4]={2,3,6,7};
					int wheelTargetVelocities[4]={-10,-10,-10,-10};
					for (int i=0;i<4;i++)
					{
						b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
						controlArgs.m_targetVelocity = wheelTargetVelocities[i];
						controlArgs.m_maxTorqueValue = 1e30;
						m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs);
					}
				}
			}
			{
				b3RobotSimulatorLoadFileResults results;
				m_robotSim.loadSDF("kiva_shelf/model.sdf",results);
			}
			{
				m_robotSim.loadURDF("plane.urdf");
			}

			m_robotSim.setGravity(b3MakeVector3(0,0,-10));
	
		}

		if ((m_options & eROBOTIC_LEARN_COMPLIANT_CONTACT)!=0)
		{
			b3RobotSimulatorLoadUrdfFileArgs args;
			b3RobotSimulatorLoadFileResults results;
			{
				args.m_startPosition.setValue(0,0,2.5);
				args.m_startOrientation.setEulerZYX(0,0.2,0);
				m_r2d2Index = m_robotSim.loadURDF("cube_soft.urdf",args);
			}
			{
				args.m_startPosition.setValue(0,2,2.5);
				args.m_startOrientation.setEulerZYX(0,0.2,0);
				m_robotSim.loadURDF("cube_no_friction.urdf",args);
			}
			{
				args.m_startPosition.setValue(0,0,0);
				args.m_startOrientation.setEulerZYX(0,0.2,0);
				args.m_forceOverrideFixedBase = true;
				m_robotSim.loadURDF("plane.urdf",args);
			}

			m_robotSim.setGravity(b3MakeVector3(0,0,-10));
		}
	
        if ((m_options & eROBOTIC_LEARN_ROLLING_FRICTION)!=0)
        {
            b3RobotSimulatorLoadUrdfFileArgs args;
			b3RobotSimulatorLoadFileResults results;
            {
                args.m_startPosition.setValue(0,0,2.5);
                args.m_startOrientation.setEulerZYX(0,0,0);
                args.m_useMultiBody = true;
                m_robotSim.loadURDF("sphere2_rolling_friction.urdf",args);
            }
            {
                args.m_startPosition.setValue(0,2,2.5);
                args.m_startOrientation.setEulerZYX(0,0,0);
                args.m_useMultiBody = true;
                m_robotSim.loadURDF("sphere2.urdf", args);
            }
            {
                args.m_startPosition.setValue(0,0,0);
                args.m_startOrientation.setEulerZYX(0,0.2,0);
                args.m_useMultiBody = true;
                args.m_forceOverrideFixedBase = true;
                m_robotSim.loadURDF("plane.urdf", args);
            }

			m_robotSim.setGravity(b3MakeVector3(0,0,-10));
		}

		
    }