bool SkeletonModel::getEyeModelPositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const {
if (!isActive()) {
return false;
}
const FBXGeometry& geometry = getFBXGeometry();
if (getJointPosition(geometry.leftEyeJointIndex, firstEyePosition) &&
getJointPosition(geometry.rightEyeJointIndex, secondEyePosition)) {
return true;
}
// no eye joints; try to estimate based on head/neck joints
glm::vec3 neckPosition, headPosition;
if (getJointPosition(geometry.neckJointIndex, neckPosition) &&
getJointPosition(geometry.headJointIndex, headPosition)) {
const float EYE_PROPORTION = 0.6f;
glm::vec3 baseEyePosition = glm::mix(neckPosition, headPosition, EYE_PROPORTION);
glm::quat headRotation;
getJointRotation(geometry.headJointIndex, headRotation);
const float EYES_FORWARD = 0.25f;
const float EYE_SEPARATION = 0.1f;
float headHeight = glm::distance(neckPosition, headPosition);
firstEyePosition = baseEyePosition + headRotation * glm::vec3(EYE_SEPARATION, 0.0f, EYES_FORWARD) * headHeight;
secondEyePosition = baseEyePosition + headRotation * glm::vec3(-EYE_SEPARATION, 0.0f, EYES_FORWARD) * headHeight;
return true;
}
return false;
}
bool SkeletonModel::getEyeModelPositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const {
if (!isActive()) {
return false;
}
if (getJointPosition(_rig.indexOfJoint("LeftEye"), firstEyePosition) &&
getJointPosition(_rig.indexOfJoint("RightEye"), secondEyePosition)) {
return true;
}
int headJointIndex = _rig.indexOfJoint("Head");
glm::vec3 headPosition;
if (getJointPosition(headJointIndex, headPosition)) {
// get head joint rotation.
glm::quat headRotation;
getJointRotation(headJointIndex, headRotation);
float heightRatio = _rig.getUnscaledEyeHeight() / DEFAULT_AVATAR_EYE_HEIGHT;
glm::vec3 ipdOffset = glm::vec3(DEFAULT_AVATAR_IPD / 2.0f, 0.0f, 0.0f);
firstEyePosition = headPosition + headRotation * heightRatio * (DEFAULT_AVATAR_HEAD_TO_MIDDLE_EYE_OFFSET + ipdOffset);
secondEyePosition = headPosition + headRotation * heightRatio * (DEFAULT_AVATAR_HEAD_TO_MIDDLE_EYE_OFFSET - ipdOffset);
return true;
}
return false;
}
void SkeletonModel::applyHandPosition(int jointIndex, const glm::vec3& position) {
if (jointIndex == -1 || jointIndex >= _rig->getJointStateCount()) {
return;
}
// NOTE: 'position' is in model-frame
setJointPosition(jointIndex, position, glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
const FBXGeometry& geometry = _geometry->getFBXGeometry();
glm::vec3 handPosition, elbowPosition;
getJointPosition(jointIndex, handPosition);
getJointPosition(geometry.joints.at(jointIndex).parentIndex, elbowPosition);
glm::vec3 forearmVector = handPosition - elbowPosition;
float forearmLength = glm::length(forearmVector);
if (forearmLength < EPSILON) {
return;
}
glm::quat handRotation;
if (!_rig->getJointStateRotation(jointIndex, handRotation)) {
return;
}
// align hand with forearm
float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
_rig->applyJointRotationDelta(jointIndex,
rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector),
true, PALM_PRIORITY);
}
/// \return offset of hips after foot animation
void SkeletonModel::updateStandingFoot() {
if (_geometry == NULL) {
return;
}
glm::vec3 offset(0.0f);
int leftFootIndex = _geometry->getFBXGeometry().leftToeJointIndex;
int rightFootIndex = _geometry->getFBXGeometry().rightToeJointIndex;
if (leftFootIndex != -1 && rightFootIndex != -1) {
glm::vec3 leftPosition, rightPosition;
getJointPosition(leftFootIndex, leftPosition);
getJointPosition(rightFootIndex, rightPosition);
int lowestFoot = (leftPosition.y < rightPosition.y) ? LEFT_FOOT : RIGHT_FOOT;
const float MIN_STEP_HEIGHT_THRESHOLD = 0.05f;
bool oneFoot = fabsf(leftPosition.y - rightPosition.y) > MIN_STEP_HEIGHT_THRESHOLD;
int currentFoot = oneFoot ? lowestFoot : _standingFoot;
if (_standingFoot == NO_FOOT) {
currentFoot = lowestFoot;
}
if (currentFoot != _standingFoot) {
if (_standingFoot == NO_FOOT) {
// pick the lowest foot
glm::vec3 lowestPosition = (currentFoot == LEFT_FOOT) ? leftPosition : rightPosition;
// we ignore zero length positions which can happen for a few frames until skeleton is fully loaded
if (glm::length(lowestPosition) > 0.0f) {
_standingFoot = currentFoot;
_clampedFootPosition = lowestPosition;
}
} else {
// swap feet
_standingFoot = currentFoot;
glm::vec3 nextPosition = leftPosition;
glm::vec3 prevPosition = rightPosition;
if (_standingFoot == RIGHT_FOOT) {
nextPosition = rightPosition;
prevPosition = leftPosition;
}
glm::vec3 oldOffset = _clampedFootPosition - prevPosition;
_clampedFootPosition = oldOffset + nextPosition;
offset = _clampedFootPosition - nextPosition;
}
} else {
glm::vec3 nextPosition = (_standingFoot == LEFT_FOOT) ? leftPosition : rightPosition;
offset = _clampedFootPosition - nextPosition;
}
// clamp the offset to not exceed some max distance
const float MAX_STEP_OFFSET = 1.0f;
float stepDistance = glm::length(offset);
if (stepDistance > MAX_STEP_OFFSET) {
offset *= (MAX_STEP_OFFSET / stepDistance);
}
}
_standingOffset = offset;
}
void SkeletonModel::setJointStates(QVector<JointState> states) {
Model::setJointStates(states);
// Determine the default eye position for avatar scale = 1.0
int headJointIndex = _geometry->getFBXGeometry().headJointIndex;
if (0 <= headJointIndex && headJointIndex < _jointStates.size()) {
glm::vec3 leftEyePosition, rightEyePosition;
getEyeModelPositions(leftEyePosition, rightEyePosition);
glm::vec3 midEyePosition = (leftEyePosition + rightEyePosition) / 2.0f;
int rootJointIndex = _geometry->getFBXGeometry().rootJointIndex;
glm::vec3 rootModelPosition;
getJointPosition(rootJointIndex, rootModelPosition);
_defaultEyeModelPosition = midEyePosition - rootModelPosition;
_defaultEyeModelPosition.z = -_defaultEyeModelPosition.z;
// Skeleton may have already been scaled so unscale it
_defaultEyeModelPosition = _defaultEyeModelPosition / _scale;
}
// the SkeletonModel override of updateJointState() will clear the translation part
// of its root joint and we need that done before we try to build shapes hence we
// recompute all joint transforms at this time.
for (int i = 0; i < _jointStates.size(); i++) {
updateJointState(i);
}
clearShapes();
if (_enableShapes) {
buildShapes();
}
}
void SkeletonModel::initJointStates() {
const FBXGeometry& geometry = getFBXGeometry();
glm::mat4 modelOffset = glm::scale(_scale) * glm::translate(_offset);
_rig->initJointStates(geometry, modelOffset);
// Determine the default eye position for avatar scale = 1.0
int headJointIndex = geometry.headJointIndex;
if (0 > headJointIndex || headJointIndex >= _rig->getJointStateCount()) {
qCWarning(interfaceapp) << "Bad head joint! Got:" << headJointIndex << "jointCount:" << _rig->getJointStateCount();
}
glm::vec3 leftEyePosition, rightEyePosition;
getEyeModelPositions(leftEyePosition, rightEyePosition);
glm::vec3 midEyePosition = (leftEyePosition + rightEyePosition) / 2.0f;
int rootJointIndex = geometry.rootJointIndex;
glm::vec3 rootModelPosition;
getJointPosition(rootJointIndex, rootModelPosition);
_defaultEyeModelPosition = midEyePosition - rootModelPosition;
// Skeleton may have already been scaled so unscale it
_defaultEyeModelPosition = _defaultEyeModelPosition / _scale;
computeBoundingShape();
Extents meshExtents = getMeshExtents();
_headClipDistance = -(meshExtents.minimum.z / _scale.z - _defaultEyeModelPosition.z);
_headClipDistance = std::max(_headClipDistance, DEFAULT_NEAR_CLIP);
_owningAvatar->rebuildCollisionShape();
emit skeletonLoaded();
}
void SkeletonModel::initJointStates(QVector<JointState> states) {
const FBXGeometry& geometry = _geometry->getFBXGeometry();
glm::mat4 parentTransform = glm::scale(_scale) * glm::translate(_offset) * geometry.offset;
int rootJointIndex = geometry.rootJointIndex;
int leftHandJointIndex = geometry.leftHandJointIndex;
int leftElbowJointIndex = leftHandJointIndex >= 0 ? geometry.joints.at(leftHandJointIndex).parentIndex : -1;
int leftShoulderJointIndex = leftElbowJointIndex >= 0 ? geometry.joints.at(leftElbowJointIndex).parentIndex : -1;
int rightHandJointIndex = geometry.rightHandJointIndex;
int rightElbowJointIndex = rightHandJointIndex >= 0 ? geometry.joints.at(rightHandJointIndex).parentIndex : -1;
int rightShoulderJointIndex = rightElbowJointIndex >= 0 ? geometry.joints.at(rightElbowJointIndex).parentIndex : -1;
_boundingRadius = _rig->initJointStates(states, parentTransform,
rootJointIndex,
leftHandJointIndex,
leftElbowJointIndex,
leftShoulderJointIndex,
rightHandJointIndex,
rightElbowJointIndex,
rightShoulderJointIndex);
// Determine the default eye position for avatar scale = 1.0
int headJointIndex = _geometry->getFBXGeometry().headJointIndex;
if (0 <= headJointIndex && headJointIndex < _rig->getJointStateCount()) {
glm::vec3 leftEyePosition, rightEyePosition;
getEyeModelPositions(leftEyePosition, rightEyePosition);
glm::vec3 midEyePosition = (leftEyePosition + rightEyePosition) / 2.0f;
int rootJointIndex = _geometry->getFBXGeometry().rootJointIndex;
glm::vec3 rootModelPosition;
getJointPosition(rootJointIndex, rootModelPosition);
_defaultEyeModelPosition = midEyePosition - rootModelPosition;
_defaultEyeModelPosition.z = -_defaultEyeModelPosition.z;
// Skeleton may have already been scaled so unscale it
_defaultEyeModelPosition = _defaultEyeModelPosition / _scale;
}
// the SkeletonModel override of updateJointState() will clear the translation part
// of its root joint and we need that done before we try to build shapes hence we
// recompute all joint transforms at this time.
for (int i = 0; i < _rig->getJointStateCount(); i++) {
_rig->updateJointState(i, parentTransform);
}
buildShapes();
Extents meshExtents = getMeshExtents();
_headClipDistance = -(meshExtents.minimum.z / _scale.z - _defaultEyeModelPosition.z);
_headClipDistance = std::max(_headClipDistance, DEFAULT_NEAR_CLIP);
_owningAvatar->rebuildSkeletonBody();
emit skeletonLoaded();
}
void RobotVREP::addJoint( Joint * joint)
{
int * handler = new int(getObjectHandle(joint->getName(), simx_opmode_oneshot_wait));
VrepHandlerVector.push_back( handler );
jointVector.push_back( joint );
jointIdToVrepHandler_map.insert( std::pair<int,int> ( joint->getUniqueObjectId() , VrepHandlerVector.size() -1 ) );
getJointForce(joint, simx_opmode_streaming);
getJointPosition(joint, simx_opmode_streaming);
}
void SkeletonModel::applyHandPosition(int jointIndex, const glm::vec3& position) {
if (jointIndex == -1) {
return;
}
setJointPosition(jointIndex, position);
const FBXGeometry& geometry = _geometry->getFBXGeometry();
glm::vec3 handPosition, elbowPosition;
getJointPosition(jointIndex, handPosition);
getJointPosition(geometry.joints.at(jointIndex).parentIndex, elbowPosition);
glm::vec3 forearmVector = handPosition - elbowPosition;
float forearmLength = glm::length(forearmVector);
if (forearmLength < EPSILON) {
return;
}
glm::quat handRotation;
getJointRotation(jointIndex, handRotation, true);
// align hand with forearm
float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
applyRotationDelta(jointIndex, rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), false);
}
void SkeletonModel::initJointStates(QVector<JointState> states) {
Model::initJointStates(states);
// Determine the default eye position for avatar scale = 1.0
int headJointIndex = _geometry->getFBXGeometry().headJointIndex;
if (0 <= headJointIndex && headJointIndex < _jointStates.size()) {
glm::vec3 leftEyePosition, rightEyePosition;
getEyeModelPositions(leftEyePosition, rightEyePosition);
glm::vec3 midEyePosition = (leftEyePosition + rightEyePosition) / 2.0f;
int rootJointIndex = _geometry->getFBXGeometry().rootJointIndex;
glm::vec3 rootModelPosition;
getJointPosition(rootJointIndex, rootModelPosition);
_defaultEyeModelPosition = midEyePosition - rootModelPosition;
_defaultEyeModelPosition.z = -_defaultEyeModelPosition.z;
// Skeleton may have already been scaled so unscale it
_defaultEyeModelPosition = _defaultEyeModelPosition / _scale;
}
// the SkeletonModel override of updateJointState() will clear the translation part
// of its root joint and we need that done before we try to build shapes hence we
// recompute all joint transforms at this time.
for (int i = 0; i < _jointStates.size(); i++) {
updateJointState(i);
}
clearShapes();
if (_enableShapes) {
buildShapes();
}
Extents meshExtents = getMeshExtents();
_headClipDistance = -(meshExtents.minimum.z / _scale.z - _defaultEyeModelPosition.z);
_headClipDistance = std::max(_headClipDistance, DEFAULT_NEAR_CLIP);
_owningAvatar->rebuildSkeletonBody();
emit skeletonLoaded();
}
void SkeletonModel::initJointStates() {
const HFMModel& hfmModel = getHFMModel();
glm::mat4 modelOffset = glm::scale(_scale) * glm::translate(_offset);
_rig.initJointStates(hfmModel, modelOffset);
{
// initialize _jointData with proper values for default joints
QVector<JointData> defaultJointData;
_rig.copyJointsIntoJointData(defaultJointData);
_owningAvatar->setRawJointData(defaultJointData);
}
// Determine the default eye position for avatar scale = 1.0
int headJointIndex = _rig.indexOfJoint("Head");
if (0 > headJointIndex || headJointIndex >= _rig.getJointStateCount()) {
qCWarning(avatars_renderer) << "Bad head joint! Got:" << headJointIndex << "jointCount:" << _rig.getJointStateCount();
}
glm::vec3 leftEyePosition, rightEyePosition;
getEyeModelPositions(leftEyePosition, rightEyePosition);
glm::vec3 midEyePosition = (leftEyePosition + rightEyePosition) / 2.0f;
int rootJointIndex = _rig.indexOfJoint("Hips");
glm::vec3 rootModelPosition;
getJointPosition(rootJointIndex, rootModelPosition);
_defaultEyeModelPosition = midEyePosition - rootModelPosition;
// Skeleton may have already been scaled so unscale it
_defaultEyeModelPosition = _defaultEyeModelPosition / _scale;
computeBoundingShape();
Extents meshExtents = getMeshExtents();
_headClipDistance = -(meshExtents.minimum.z / _scale.z - _defaultEyeModelPosition.z);
_headClipDistance = std::max(_headClipDistance, DEFAULT_NEAR_CLIP);
_owningAvatar->rebuildCollisionShape();
emit skeletonLoaded();
}
void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation) {
// this algorithm is from sample code from sixense
const FBXGeometry& geometry = _geometry->getFBXGeometry();
int elbowJointIndex = geometry.joints.at(jointIndex).parentIndex;
if (elbowJointIndex == -1) {
return;
}
int shoulderJointIndex = geometry.joints.at(elbowJointIndex).parentIndex;
glm::vec3 shoulderPosition;
if (!getJointPosition(shoulderJointIndex, shoulderPosition)) {
return;
}
// precomputed lengths
float scale = extractUniformScale(_scale);
float upperArmLength = geometry.joints.at(elbowJointIndex).distanceToParent * scale;
float lowerArmLength = geometry.joints.at(jointIndex).distanceToParent * scale;
// first set wrist position
glm::vec3 wristPosition = position;
glm::vec3 shoulderToWrist = wristPosition - shoulderPosition;
float distanceToWrist = glm::length(shoulderToWrist);
// prevent gimbal lock
if (distanceToWrist > upperArmLength + lowerArmLength - EPSILON) {
distanceToWrist = upperArmLength + lowerArmLength - EPSILON;
shoulderToWrist = glm::normalize(shoulderToWrist) * distanceToWrist;
wristPosition = shoulderPosition + shoulderToWrist;
}
// cosine of angle from upper arm to hand vector
float cosA = (upperArmLength * upperArmLength + distanceToWrist * distanceToWrist - lowerArmLength * lowerArmLength) /
(2 * upperArmLength * distanceToWrist);
float mid = upperArmLength * cosA;
float height = sqrt(upperArmLength * upperArmLength + mid * mid - 2 * upperArmLength * mid * cosA);
// direction of the elbow
glm::vec3 handNormal = glm::cross(rotation * glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow rotating with wrist
glm::vec3 relaxedNormal = glm::cross(glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow pointing straight down
const float NORMAL_WEIGHT = 0.5f;
glm::vec3 finalNormal = glm::mix(relaxedNormal, handNormal, NORMAL_WEIGHT);
bool rightHand = (jointIndex == geometry.rightHandJointIndex);
if (rightHand ? (finalNormal.y > 0.0f) : (finalNormal.y < 0.0f)) {
finalNormal.y = 0.0f; // dont allow elbows to point inward (y is vertical axis)
}
glm::vec3 tangent = glm::normalize(glm::cross(shoulderToWrist, finalNormal));
// ik solution
glm::vec3 elbowPosition = shoulderPosition + glm::normalize(shoulderToWrist) * mid - tangent * height;
glm::vec3 forwardVector(rightHand ? -1.0f : 1.0f, 0.0f, 0.0f);
glm::quat shoulderRotation = rotationBetween(forwardVector, elbowPosition - shoulderPosition);
JointState& shoulderState = _jointStates[shoulderJointIndex];
shoulderState.setRotationInBindFrame(shoulderRotation, PALM_PRIORITY);
JointState& elbowState = _jointStates[elbowJointIndex];
elbowState.setRotationInBindFrame(rotationBetween(shoulderRotation * forwardVector, wristPosition - elbowPosition) * shoulderRotation, PALM_PRIORITY);
JointState& handState = _jointStates[jointIndex];
handState.setRotationInBindFrame(rotation, PALM_PRIORITY);
}
bool SkeletonModel::getLocalNeckPosition(glm::vec3& neckPosition) const {
return isActive() && getJointPosition(_rig.indexOfJoint("Neck"), neckPosition);
}
bool SkeletonModel::getLocalNeckPosition(glm::vec3& neckPosition) const {
return isActive() && getJointPosition(getFBXGeometry().neckJointIndex, neckPosition);
}
void robot::createSensors()
{
std::string txt("There are "+boost::lexical_cast<std::string>(vSensors.size())+" sensors.");
printToConsole(txt.c_str());
for(size_t i = 0; i < vSensors.size() ; i++)
{
sensor *Sensor = vSensors.at(i);
if (Sensor->gazeboSpec)
printToConsole("ERROR: sensor will not be created: the URDF specification is supported, but this is a Gazebo tag which is not documented as it seems.");
else
{
if (Sensor->cameraSensorPresent)
{
int intParams[4]={Sensor->resolution[0],Sensor->resolution[1],0,0};
float floatParams[11]={Sensor->clippingPlanes[0],Sensor->clippingPlanes[1],60.0f*piValue/180.0f,0.2f,0.2f,0.4f,0.0f,0.0f,0.0f,0.0f,0.0f};
Sensor->nSensor=simCreateVisionSensor(1,intParams,floatParams,NULL);
//Set the name:
setVrepObjectName(Sensor->nSensor,std::string(name+"_camera").c_str());
}
int proxSensHandle=-1;
if (Sensor->proximitySensorPresent)
{ // Proximity sensors seem to be very very specific and not general / generic at all. How come?! I.e. a succession of ray description (with min/max distances) would do
int intParams[8]={16,16,1,4,16,1,0,0};
float floatParams[15]={0.0f,0.48f,0.1f,0.1f,0.1f,0.1f,0.0f,0.02f,0.02f,30.0f*piValue/180.0f,piValue/2.0f,0.0f,0.02f,0.0f,0.0f};
proxSensHandle=simCreateProximitySensor(sim_proximitysensor_cone_subtype,sim_objectspecialproperty_detectable_all,0,intParams,floatParams,NULL);
//Set the name:
setVrepObjectName(proxSensHandle,std::string(Sensor->name+"_proximity").c_str());
}
// the doc doesn't state if a vision and proximity sensor can be declared at the same time...
if (proxSensHandle!=-1)
{
if (Sensor->nSensor!=-1)
{
Sensor->nSensorAux=proxSensHandle;
simSetObjectParent(Sensor->nSensorAux,Sensor->nSensor,true);
}
else
Sensor->nSensor=proxSensHandle;
}
// Find the local configuration:
C7Vector sensorLocal;
sensorLocal.X.set(Sensor->origin_xyz);
sensorLocal.Q=getQuaternionFromRpy(Sensor->origin_rpy);
C4Vector rot(0.0f,0.0f,piValue); // the V-REP sensors are rotated by 180deg around the Z-axis
sensorLocal.Q=sensorLocal.Q*rot;
// We attach the sensor to a link:
C7Vector x;
x.setIdentity();
int parentLinkIndex=getLinkPosition(Sensor->parentLink);
if (parentLinkIndex!=-1)
{
int parentJointLinkIndex=getJointPosition(vLinks.at(parentLinkIndex)->parent);
if (parentJointLinkIndex!=-1)
x=vJoints.at(parentJointLinkIndex)->jointBaseFrame;
}
C7Vector sensorGlobal(x*sensorLocal);
if (Sensor->nSensor!=-1)
{
simSetObjectPosition(Sensor->nSensor,-1,sensorGlobal.X.data);
simSetObjectOrientation(Sensor->nSensor,-1,sensorGlobal.Q.getEulerAngles().data);
}
if ((parentLinkIndex!=-1)&&(Sensor->nSensor!=-1))
{
if (vLinks.at(parentLinkIndex)->visuals.size()!=0)
simSetObjectParent(Sensor->nSensor,vLinks.at(parentLinkIndex)->nLinkVisual,true);
if (vLinks.at(parentLinkIndex)->nLinkCollision!=-1)
simSetObjectParent(Sensor->nSensor,vLinks.at(parentLinkIndex)->nLinkCollision,true);
}
}
}
}
void robot::createLinks(bool hideCollisionLinks,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool showConvexDecompositionDlg)
{
std::string txt("There are "+boost::lexical_cast<std::string>(vLinks.size())+" links.");
printToConsole(txt.c_str());
for(size_t i = 0; i < vLinks.size() ; i++)
{
urdfLink *Link = vLinks.at(i);
Link->createLink(hideCollisionLinks,convexDecomposeNonConvexCollidables,createVisualIfNone,showConvexDecompositionDlg);
// We attach the collision link to a joint. If the collision link isn't there, we use the visual link instead:
C7Vector linkInitialConf;
C7Vector linkDesiredConf;
int effectiveLinkHandle=-1;
if (Link->nLinkCollision!=-1)
{
effectiveLinkHandle=Link->nLinkCollision;
if (effectiveLinkHandle != -1) {
// Visual object position and orientation is already set in the Link
float xyz[3] = {0,0,0};
float rpy[3] = {0,0,0};
linkDesiredConf.X.set(xyz);
linkDesiredConf.Q=getQuaternionFromRpy(rpy);
}
}
else
{
effectiveLinkHandle = Link->nLinkVisual;
if (effectiveLinkHandle != -1) {
// Visual object position and orientation is already set in the Link
float xyz[3] = {0,0,0};
float rpy[3] = {0,0,0};
linkDesiredConf.X.set(xyz);
linkDesiredConf.Q=getQuaternionFromRpy(rpy);
}
}
simGetObjectPosition(effectiveLinkHandle,-1,linkInitialConf.X.data);
C3Vector euler;
simGetObjectOrientation(effectiveLinkHandle,-1,euler.data);
linkInitialConf.Q.setEulerAngles(euler);
// C7Vector trAbs(linkDesiredConf*linkInitialConf); // still local
C7Vector trAbs(linkInitialConf);
int parentJointIndex=getJointPosition(Link->parent);
if( parentJointIndex!= -1)
{
joint* Joint = vJoints.at(parentJointIndex);
trAbs=Joint->jointBaseFrame*trAbs; // now absolute
}
//set the transfrom matrix to the object
simSetObjectPosition(effectiveLinkHandle,-1,trAbs.X.data);
simSetObjectOrientation(effectiveLinkHandle,-1,trAbs.Q.getEulerAngles().data);
}
// Finally the real parenting:
for(size_t i = 0; i < vJoints.size() ; i++)
{
int parentLinkIndex=getLinkPosition(vJoints.at(i)->parentLink);
if (parentLinkIndex!=-1)
{
urdfLink* parentLink=vLinks[parentLinkIndex];
if (parentLink->nLinkCollision!=-1)
simSetObjectParent(vJoints.at(i)->nJoint,parentLink->nLinkCollision,true);
else
simSetObjectParent(vJoints.at(i)->nJoint,parentLink->nLinkVisual,true);
}
int childLinkIndex=getLinkPosition(vJoints.at(i)->childLink);
if (childLinkIndex!=-1)
{
urdfLink* childLink=vLinks[childLinkIndex];
if (childLink->nLinkCollision!=-1)
simSetObjectParent(childLink->nLinkCollision,vJoints.at(i)->nJoint,true);
else
simSetObjectParent(childLink->nLinkVisual,vJoints.at(i)->nJoint,true);
}
}
}
void robot::createJoints(bool hideJoints,bool positionCtrl)
{
std::string txt("There are "+boost::lexical_cast<std::string>(vJoints.size())+" joints.");
printToConsole(txt.c_str());
//Set parents and childs for all the links
for(size_t i = 0; i < vJoints.size() ; i++)
{
vLinks.at(getLinkPosition(vJoints.at(i)->parentLink))->child = vJoints.at(i)->name;
vLinks.at(getLinkPosition(vJoints.at(i)->childLink))->parent = vJoints.at(i)->name;
}
//Create the joints
for(size_t i = 0; i < vJoints.size() ; i++)
{
//Move the joints to the positions specifieds by the urdf file
C7Vector tmp;
tmp.setIdentity();
tmp.X.set(vJoints.at(i)->origin_xyz);
tmp.Q=getQuaternionFromRpy(vJoints.at(i)->origin_rpy);
vJoints.at(i)->jointBaseFrame=vJoints.at(i)->jointBaseFrame*tmp;
//Set name jointParent to each joint
int nParentLink = getLinkPosition(vJoints.at(i)->parentLink);
vJoints.at(i)->parentJoint = vLinks.at(nParentLink)->parent;
//Create the joint/forceSensor/dummy:
if (vJoints.at(i)->jointType==-1)
vJoints.at(i)->nJoint = simCreateDummy(0.02f,NULL); // when joint type was not recognized
if (vJoints.at(i)->jointType==0)
vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
if (vJoints.at(i)->jointType==1)
vJoints.at(i)->nJoint = simCreateJoint(sim_joint_prismatic_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
if (vJoints.at(i)->jointType==2)
vJoints.at(i)->nJoint = simCreateJoint(sim_joint_spherical_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
if (vJoints.at(i)->jointType==3)
vJoints.at(i)->nJoint = simCreateJoint(sim_joint_revolute_subtype,sim_jointmode_force,2,NULL,NULL,NULL);
if (vJoints.at(i)->jointType==4)
{ // when joint type is "fixed"
int intParams[5]={1,4,4,0,0};
float floatParams[5]={0.02f,1.0f,1.0f,0.0f,0.0f};
vJoints.at(i)->nJoint = simCreateForceSensor(0,intParams,floatParams,NULL);
}
if ( (vJoints.at(i)->jointType==0)||(vJoints.at(i)->jointType==1) )
{
float interval[2]={vJoints.at(i)->lowerLimit,vJoints.at(i)->upperLimit-vJoints.at(i)->lowerLimit};
simSetJointInterval(vJoints.at(i)->nJoint,0,interval);
if (vJoints.at(i)->jointType==0)
{ // revolute
simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitAngular);
simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitAngular);
}
else
{ // prismatic
simSetJointForce(vJoints.at(i)->nJoint,vJoints.at(i)->effortLimitLinear);
simSetObjectFloatParameter(vJoints.at(i)->nJoint,2017,vJoints.at(i)->velocityLimitLinear);
}
// We turn the position control on:
if (positionCtrl)
{
simSetObjectIntParameter(vJoints.at(i)->nJoint,2000,1);
simSetObjectIntParameter(vJoints.at(i)->nJoint,2001,1);
}
}
//Set the name:
setVrepObjectName(vJoints.at(i)->nJoint,vJoints.at(i)->name.c_str());
if (hideJoints)
simSetObjectIntParameter(vJoints.at(i)->nJoint,10,512); // layer 10
}
//Set positions to joints from the 4x4matrix
for(size_t i = 0; i < vJoints.size() ; i++)
{
simSetObjectPosition(vJoints.at(i)->nJoint,-1,vJoints.at(i)->jointBaseFrame.X.data);
simSetObjectOrientation(vJoints.at(i)->nJoint,-1 ,vJoints.at(i)->jointBaseFrame.Q.getEulerAngles().data);
}
//Set joint parentship between them (thes parentship will be remove before adding the joints)
for(size_t i = 0; i < vJoints.size() ; i++)
{
int parentJointIndex=getJointPosition(vJoints.at(i)->parentJoint);
if ( parentJointIndex!= -1)
{
simInt nParentJoint = vJoints.at(parentJointIndex)->nJoint;
simInt nJoint = vJoints.at(i)->nJoint;
simSetObjectParent(nJoint,nParentJoint,false);
}
}
//Delete all the partnership without moving the joints but after doing that update the transform matrix
for(size_t i = 0; i < vJoints.size() ; i++)
{
C4X4Matrix tmp;
simGetObjectPosition(vJoints.at(i)->nJoint,-1,tmp.X.data);
C3Vector euler;
simGetObjectOrientation(vJoints.at(i)->nJoint,-1,euler.data);
tmp.M.setEulerAngles(euler);
vJoints.at(i)->jointBaseFrame = tmp;
simInt nJoint = vJoints.at(i)->nJoint;
simSetObjectParent(nJoint,-1,true);
}
for(size_t i = 0; i < vJoints.size() ; i++)
{
C4X4Matrix jointAxisMatrix;
jointAxisMatrix.setIdentity();
C3Vector axis(vJoints.at(i)->axis);
C3Vector rotAxis;
float rotAngle=0.0f;
if (axis(2)<1.0f)
{
if (axis(2)<=-1.0f)
rotAngle=3.14159265359f;
else
rotAngle=acosf(axis(2));
rotAxis(0)=-axis(1);
rotAxis(1)=axis(0);
rotAxis(2)=0.0f;
rotAxis.normalize();
C7Vector m(jointAxisMatrix);
float alpha=-atan2(rotAxis(1),rotAxis(0));
float beta=atan2(-sqrt(rotAxis(0)*rotAxis(0)+rotAxis(1)*rotAxis(1)),rotAxis(2));
C7Vector r;
r.X.clear();
r.Q.setEulerAngles(0.0f,0.0f,alpha);
m=r*m;
r.Q.setEulerAngles(0.0f,beta,0.0f);
m=r*m;
r.Q.setEulerAngles(0.0f,0.0f,rotAngle);
m=r*m;
r.Q.setEulerAngles(0.0f,-beta,0.0f);
m=r*m;
r.Q.setEulerAngles(0.0f,0.0f,-alpha);
m=r*m;
jointAxisMatrix=m.getMatrix();
}
C4Vector q((vJoints.at(i)->jointBaseFrame*jointAxisMatrix).Q);
simSetObjectOrientation(vJoints.at(i)->nJoint,-1,q.getEulerAngles().data);
}
}
