void RobotVREP::setObjectOrientation(Object * object, vector < double > orientation, int relativeTo, simxInt operationMode)
{
int uniqueId = object->getUniqueObjectId();
int vrepHandlerVectorPosition = objectIdToVrepHandler_map.at(uniqueId);
int * handler = VrepHandlerVector.at(vrepHandlerVectorPosition);
float * ori = new float[3];
ori[0] = (float)orientation.at(0);
ori[1] = (float)orientation.at(1);
ori[2] = (float)orientation.at(2);
int error = simxSetObjectOrientation(clientID, *handler, relativeTo, ori, operationMode);
if(error != 0) vrep_error << "simxSetObjectOrientation: " << *handler << " : " << error << endl;
delete[] ori;
}
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();
}
