void Object::setGlobalPosition(std::tuple<float, float, float>& position) {
int N = 3;
simxFloat positionVec[N];
positionVec[0] = std::get<0>(position);
positionVec[1] = std::get<1>(position);
positionVec[2] = std::get<2>(position);
try {
ReturnCodesExceptions::handleReturnCodeNoResponse(
simxSetObjectPosition(m_ClientId, m_Handle, -1, positionVec, simx_opmode_oneshot));
}
catch (ReturnCodesExceptions *ex) {
throw ex;
}
}
void RobotVREP::setObjectPosition(Object * object, vector < double > position, int relativeTo, simxInt operationMode)
{
int uniqueId = object->getUniqueObjectId();
int vrepHandlerVectorPosition = objectIdToVrepHandler_map.at(uniqueId);
int * handler = VrepHandlerVector.at(vrepHandlerVectorPosition);
float * pos = new float[3];
pos[0] = (float)position.at(0);
pos[1] = (float)position.at(1);
pos[2] = (float)position.at(2);
int error = simxSetObjectPosition(clientID, *handler, relativeTo, pos, operationMode);
if(error != 0) vrep_error << "simxSetObjectPosition: " << *handler << " : " << error << endl;
delete[] pos;
}
void YouBotBaseService::update()
{
if(is_in_visualization_mode)
{
in_odometry_state.read(m_odometry_state);
simxFloat pos[3];
pos[0] = m_odometry_state.pose.pose.position.x;
pos[1] = m_odometry_state.pose.pose.position.y;
pos[2] = m_odometry_state.pose.pose.position.z;
simxSetObjectPosition(clientID,all_robot_handle,-1,pos,simx_opmode_oneshot);
double euler[3];
simxFloat euler_s[3];
//different coordinate systems, fixing it here
KDL::Rotation orientation = KDL::Rotation::Quaternion(
m_odometry_state.pose.pose.orientation.x,
m_odometry_state.pose.pose.orientation.y,
m_odometry_state.pose.pose.orientation.z,
m_odometry_state.pose.pose.orientation.w);
// Instead of transforming for the inverse of this rotation,
// we should find the right transform. (this is legacy code that
// transforms vrep coords to rviz coords)
KDL::Rotation rotation = KDL::Rotation::RPY(0,-M_PI/2,M_PI).Inverse();
orientation = orientation * rotation;
orientation.GetRPY(euler[0],euler[1],euler[2]);
euler_s[0] = euler[0];
euler_s[1] = euler[1];
euler_s[2] = euler[2];
simxSetObjectOrientation(clientID,all_robot_handle,-1,euler_s,simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[0],m_joint_state.position[0], simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[1],m_joint_state.position[1], simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[2],m_joint_state.position[2], simx_opmode_oneshot);
simxSetJointPosition(clientID,vrep_joint_handle[3],m_joint_state.position[3], simx_opmode_oneshot);
return;
}
Hilas::IRobotBaseService::update();
}
