void YouBotBaseService::setTwistSetpoints()
{
cmd_twist.read(m_cmd_twist);
std::vector<quantity<angular_velocity> > wheelVelocities;
quantity<si::velocity> longitudinalVelocity;
quantity<si::velocity> transversalVelocity;
quantity<si::angular_velocity> angularVelocity;
longitudinalVelocity = m_cmd_twist.linear.x * meter_per_second;
transversalVelocity = m_cmd_twist.linear.y * meter_per_second;
angularVelocity = m_cmd_twist.angular.z * radian_per_second;
youBotBaseKinematic.cartesianVelocityToWheelVelocities(longitudinalVelocity, transversalVelocity, angularVelocity, wheelVelocities);
// Little hack
int sign[4] = {1,-1,1,-1};
simxPauseCommunication(clientID,1);
for(unsigned int i=0; i < wheelVelocities.size() ; i++)
{
simxSetJointTargetVelocity(clientID,vrep_joint_handle[i],wheelVelocities[i].value() * sign[i], simx_opmode_oneshot);
}
simxPauseCommunication(clientID,0);
}
void RobotVREP::pauseCommunication(int action)
{
simxPauseCommunication(clientID, action);
}
void YouBotBaseService::setJointSetpoints()
{
joint_position_command.read(m_joint_position_command);
joint_velocity_command.read(m_joint_velocity_command);
joint_effort_command.read(m_joint_effort_command);
FlowStatus f = joint_position_command.read(m_joint_position_command);
FlowStatus f1 = joint_velocity_command.read(m_joint_velocity_command);
FlowStatus f2 = joint_effort_command.read(m_joint_effort_command);
simxPauseCommunication(clientID,1);
for (unsigned int joint_nr = 0; joint_nr < YouBot::NR_OF_BASE_SLAVES; ++joint_nr)
{
switch (m_joint_ctrl_modes[joint_nr])
{
case(Hilas::PLANE_ANGLE):
{
//if(f != NewData) break;
simxSetJointTargetPosition(clientID,vrep_joint_handle[joint_nr],m_joint_position_command.positions[joint_nr], simx_opmode_oneshot);
break;
}
case(Hilas::ANGULAR_VELOCITY):
{
//if(f1 != NewData) break;
// Little hack
int sign[4] = {1,-1,1,-1};
simxSetJointTargetVelocity(clientID,vrep_joint_handle[joint_nr],m_joint_velocity_command.velocities[joint_nr] * sign[joint_nr], simx_opmode_oneshot);
break;
}
case(Hilas::TORQUE):
{
//if(f2 != NewData) break;
simxSetJointForce(clientID,vrep_joint_handle[joint_nr],m_joint_effort_command.efforts[joint_nr], simx_opmode_oneshot);
break;
}
case(Hilas::MOTOR_STOP):
{
simxSetJointTargetPosition(clientID,vrep_joint_handle[0],m_joint_state.position[0], simx_opmode_oneshot);
simxSetJointTargetPosition(clientID,vrep_joint_handle[1],m_joint_state.position[1], simx_opmode_oneshot);
simxSetJointTargetPosition(clientID,vrep_joint_handle[2],m_joint_state.position[2], simx_opmode_oneshot);
simxSetJointTargetPosition(clientID,vrep_joint_handle[3],m_joint_state.position[3], simx_opmode_oneshot);
break;
}
case(Hilas::TWIST):
{
log(Error) << "Cannot be in TWIST ctrl_mode (programming error)"
<< endlog();
this->getOwner()->error();
break;
}
default:
{
log(Error) << "Case not recognized." << endlog();
this->getOwner()->error();
break;
}
}
}
simxPauseCommunication(clientID,0);
}
int main(int argc, char* argv[])
{
int portNb=0;
int motorHandles[7];
int motorHandles2[7];
if (argc>=9+7)
{ // We get the port and the motor handles (for the visual feedback, not for IK!) via command-line arguments
portNb=atoi(argv[1]);
motorHandles[0]=atoi(argv[2]);
motorHandles[1]=atoi(argv[3]);
motorHandles[2]=atoi(argv[4]);
motorHandles[3]=atoi(argv[5]);
motorHandles[4]=atoi(argv[6]);
motorHandles[5]=atoi(argv[7]);
motorHandles[6]=atoi(argv[8]);
motorHandles2[0]=atoi(argv[9]);
motorHandles2[1]=atoi(argv[10]);
motorHandles2[2]=atoi(argv[11]);
motorHandles2[3]=atoi(argv[12]);
motorHandles2[4]=atoi(argv[13]);
motorHandles2[5]=atoi(argv[14]);
motorHandles2[6]=atoi(argv[15]);
}
else
{
printf("Indicate following arguments: 'portNumber motor1Handle motor2Handle .. motor7Handle'!\n");
extApi_sleepMs(5000);
return 0;
}
// Read the kinematic file:
FILE *file;
file=fopen("lbr_iiwa_7_r800.ik","rb");
unsigned char* data=NULL;
int dataLength=0;
if (file)
{
fseek(file,0,SEEK_END);
unsigned long fl=ftell(file);
dataLength=(int)fl;
fseek(file,0,SEEK_SET);
data=new unsigned char[dataLength];
fread((char*)data,dataLength,1,file);
fclose(file);
}
else
{
printf("The kinematic content file 'lbr.ik' could not be read!\n");
extApi_sleepMs(5000);
return 0;
}
// Initialize the embedded robot model1:
int handle1=simEmbLaunch();
simEmbStart(data,dataLength);
// Initialize the embedded robot model2:
int handle2=simEmbLaunch();
simEmbStart(data,dataLength);
delete[] data;
simEmbSwitch(handle1); // use robot model1
// Connect to V-REP at the above specified port, via the remote API. V-REP is just used for visual feed-back, not IK calculation!
int clientID=simxStart("127.0.0.1",portNb,true,true,2000,5);
if (clientID!=-1)
{
float simulationStep;
simxGetFloatingParameter(clientID,sim_floatparam_simulation_time_step,&simulationStep,simx_opmode_streaming);
simxSynchronous(clientID,1); // We enable the synchronous mode, so that we can trigger each simulation step from here
int embeddedModelMotorHandles[7];
embeddedModelMotorHandles[0]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint1");
embeddedModelMotorHandles[1]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint2");
embeddedModelMotorHandles[2]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint3");
embeddedModelMotorHandles[3]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint4");
embeddedModelMotorHandles[4]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint5");
embeddedModelMotorHandles[5]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint6");
embeddedModelMotorHandles[6]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint7");
int embeddedModelTargetHandle=simEmbGetObjectHandle("LBR_iiwa_7_R800_target");
int embeddedModelBaseHandle=simEmbGetObjectHandle("LBR_iiwa_7_R800");
extIkReal v=0.0;
// Get the initial target dummy matrix, of the embedded model:
extIkReal matrix[12];
simEmbGetObjectMatrix(embeddedModelTargetHandle,embeddedModelBaseHandle,matrix);
int embeddedModelMotorHandles2[7];
embeddedModelMotorHandles2[0]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint1");
embeddedModelMotorHandles2[1]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint2");
embeddedModelMotorHandles2[2]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint3");
embeddedModelMotorHandles2[3]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint4");
embeddedModelMotorHandles2[4]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint5");
embeddedModelMotorHandles2[5]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint6");
embeddedModelMotorHandles2[6]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint7");
int embeddedModelTargetHandle2=simEmbGetObjectHandle("LBR_iiwa_7_R800_target");
int embeddedModelBaseHandle2=simEmbGetObjectHandle("LBR_iiwa_7_R800");
extIkReal v2=0.0;
// Get the initial target dummy matrix, of the embedded model:
extIkReal matrix2[12];
simEmbGetObjectMatrix(embeddedModelTargetHandle2,embeddedModelBaseHandle2,matrix2);
while (simxGetConnectionId(clientID)!=-1)
{
// Following 3 commands will slow down the simulation, but garantee that if the simulation time step was changed,
// that there won't be any jumps. Following 3 commands are not needed if you don't modify the simulation time step
// (i.e. dt) during simulation.
simxUChar vrepWaitingForTrigger=0;
while ( (vrepWaitingForTrigger==0)&&(simxGetConnectionId(clientID)!=-1) )
simxGetBooleanParameter(clientID,sim_boolparam_waiting_for_trigger,&vrepWaitingForTrigger,simx_opmode_oneshot_wait);
simxGetFloatingParameter(clientID,sim_floatparam_simulation_time_step,&simulationStep,simx_opmode_buffer);
v+=extIkReal(0.2)*extIkReal(simulationStep);
v2+=extIkReal(0.28)*extIkReal(simulationStep);
simEmbSwitch(handle1); // use robot model1
// Set the desired tip matrix by setting the target dummy matrix:
matrix[3]=extIkReal(-0.3-cos(v)*0.1);
matrix[7]=extIkReal(sin(v)*0.1);
matrix[11]=extIkReal(0.629+sin(v*9)*0.01);
simEmbSetObjectMatrix(embeddedModelTargetHandle,embeddedModelBaseHandle,matrix);
// calculate IK:
simEmbHandleIkGroup(sim_handle_all);
// Read the corresponding motor angles and send them to V-REP:
simxPauseCommunication(clientID,1); // Temporarily pause the remote API communication, in order to send all following commands at once
extIkReal pos;
simEmbGetJointPosition(embeddedModelMotorHandles[0],&pos);
simxSetJointPosition(clientID,motorHandles[0],(float)pos,simx_opmode_oneshot);
simEmbGetJointPosition(embeddedModelMotorHandles[1],&pos);
simxSetJointPosition(clientID,motorHandles[1],(float)pos,simx_opmode_oneshot);
simEmbGetJointPosition(embeddedModelMotorHandles[2],&pos);
simxSetJointPosition(clientID,motorHandles[2],(float)pos,simx_opmode_oneshot);
simEmbGetJointPosition(embeddedModelMotorHandles[3],&pos);
simxSetJointPosition(clientID,motorHandles[3],(float)pos,simx_opmode_oneshot);
simEmbGetJointPosition(embeddedModelMotorHandles[4],&pos);
simxSetJointPosition(clientID,motorHandles[4],(float)pos,simx_opmode_oneshot);
simEmbGetJointPosition(embeddedModelMotorHandles[5],&pos);
simxSetJointPosition(clientID,motorHandles[5],(float)pos,simx_opmode_oneshot);
simEmbGetJointPosition(embeddedModelMotorHandles[6],&pos);
simxSetJointPosition(clientID,motorHandles[6],(float)pos,simx_opmode_oneshot);
simEmbSwitch(handle2); // use robot model2
// Set the desired tip matrix by setting the target dummy matrix:
matrix2[3]=extIkReal(-0.3-cos(v2)*0.1);
matrix2[7]=extIkReal(sin(v2)*0.1);
simEmbSetObjectMatrix(embeddedModelTargetHandle2,embeddedModelBaseHandle2,matrix2);
// calculate IK:
simEmbHandleIkGroup(sim_handle_all);
// Read the corresponding motor angles and send them to V-REP:
simEmbGetJointPosition(embeddedModelMotorHandles2[0],&pos);
simxSetJointPosition(clientID,motorHandles2[0],(float)pos,simx_opmode_oneshot);
simEmbGetJointPosition(embeddedModelMotorHandles2[1],&pos);
simxSetJointPosition(clientID,motorHandles2[1],(float)pos,simx_opmode_oneshot);
simEmbGetJointPosition(embeddedModelMotorHandles2[2],&pos);
simxSetJointPosition(clientID,motorHandles2[2],(float)pos,simx_opmode_oneshot);
simEmbGetJointPosition(embeddedModelMotorHandles2[3],&pos);
simxSetJointPosition(clientID,motorHandles2[3],(float)pos,simx_opmode_oneshot);
simEmbGetJointPosition(embeddedModelMotorHandles2[4],&pos);
simxSetJointPosition(clientID,motorHandles2[4],(float)pos,simx_opmode_oneshot);
simEmbGetJointPosition(embeddedModelMotorHandles2[5],&pos);
simxSetJointPosition(clientID,motorHandles2[5],(float)pos,simx_opmode_oneshot);
simEmbGetJointPosition(embeddedModelMotorHandles2[6],&pos);
simxSetJointPosition(clientID,motorHandles2[6],(float)pos,simx_opmode_oneshot);
simxPauseCommunication(clientID,0); // Unpause the remote API communication
// Now step the simulation on V-REP side:
int r=simx_return_remote_error_flag; // means for next remote API function call: step not triggered
while (r==simx_return_remote_error_flag)
r=simxSynchronousTrigger(clientID); // Trigger next simulation step
if (r!=simx_return_ok)
break;
printf(".");
}
simEmbShutDown(); // End the external IK
simEmbShutDown(); // End the external IK
simxFinish(clientID); // End the remote API
}
return(0);
}
