void JointControlPluglet::positionCallback(
const brics_actuator::JointPositions msg) {
//if (ctl_state != Position) {
//ctl_state = Position;
//}
for (unsigned int i = 0; i < msg.positions.size(); i++) {
if (!controlledJoint(msg.positions[i].joint_uri)) {
continue;
}
int handle = simGetObjectHandle(msg.positions[i].joint_uri.c_str());
if (handle > 0) {
simSetObjectIntParameter(handle, 2000, 1);
simSetObjectIntParameter(handle, 2001, 1);
simSetJointTargetPosition(handle,
msg.positions[i].value);
}
}
}
void CSubscriberData::setObjectIntParameterCallback(const std_msgs::Int32::ConstPtr& param)
{
if (_handleGeneralCallback_before())
{
if (simSetObjectIntParameter(auxInt1,auxInt2,param->data)<=0)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void JointControlPluglet::velocityCallback(
const brics_actuator::JointVelocities msg) {
//ctl_state = Velocity;
//}
for (unsigned int i = 0; i < msg.velocities.size(); i++) {
if (!controlledJoint(msg.velocities[i].joint_uri)) {
continue;
}
int handle = simGetObjectHandle(msg.velocities[i].joint_uri.c_str());
if (handle > 0) {
if (std::fabs(msg.velocities[i].value) < 0.0000001) {
brics_actuator::JointPositions msg_single;
msg_single.poisonStamp = msg.poisonStamp;
brics_actuator::JointValue value = msg.velocities[i];
value.unit = "rad";
float dval = 0.0;
simGetJointPosition(handles[i], &dval);
value.value = dval;
msg_single.positions.push_back(value);
//positionCallback(msg_single);
simSetObjectIntParameter(handle, 2000, 1);
simSetObjectIntParameter(handle, 2001, 1);
simSetJointTargetPosition(handle, dval);
} else {
simSetObjectIntParameter(handle, 2000, 1);
simSetObjectIntParameter(handle, 2001, 0);
simSetObjectIntParameter(handle, 1000, 1);
simSetJointTargetVelocity(handle, static_cast<float>(msg.velocities[i].value));
}
}
}
}
void robot::setLocalRespondableMaskCummulative_alternate(int objHandle,bool bitSet)
{
if (simGetObjectType(objHandle)==sim_object_shape_type)
{
int p;
simGetObjectIntParameter(objHandle,3004,&p);
if (p!=0)
{
if (bitSet)
simSetObjectIntParameter(objHandle,3019,0xff01);
else
simSetObjectIntParameter(objHandle,3019,0xff02);
bitSet=!bitSet;
}
}
int index=0;
while (true)
{
int childHandle=simGetObjectChild(objHandle,index++);
if (childHandle==-1)
break;
setLocalRespondableMaskCummulative_alternate(childHandle,bitSet);
}
}
void JointControlPluglet::torqueCallback(
const brics_actuator::JointTorques msg) {
for (unsigned int i = 0; i < msg.torques.size(); i++) {
if (!controlledJoint(msg.torques[i].joint_uri)) {
continue;
}
int handle = simGetObjectHandle(msg.torques[i].joint_uri.c_str());
if (handle > 0) {
simSetObjectIntParameter(handle, 2000, 1);
simSetObjectIntParameter(handle, 2001, 0);
simSetObjectIntParameter(handle, 1000, 2);
simSetJointForce(handle,
msg.torques[i].value);
}
}
}
//Write
void urdfLink::createLink(bool hideCollisionLinks,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool& showConvexDecompositionDlg)
{
std::string txt("Creating link '"+name+"'...");
printToConsole(txt.c_str());
//visuals.clear();
// Visuals
for (int i=0; i<visuals.size(); i++) {
urdfElement &visual = visuals[i];
if(!visual.meshFilename.empty())
{
std::string fname(visual.meshFilename);
bool exists=true;
bool useAlt=false;
if (!simDoesFileExist(fname.c_str()))
{
fname=visual.meshFilename_alt;
exists=simDoesFileExist(fname.c_str());
useAlt=true;
}
if (!exists)
printToConsole("ERROR: the mesh file could not be found.");
else
visual.n = simImportShape(visual.meshExtension,fname.c_str(),0,0.0001f,1.0);
if (!visual.n)
{
if (!useAlt)
txt="ERROR: failed to create the mesh '"+visual.meshFilename+"' with extension type "+boost::lexical_cast<std::string>(visual.meshExtension);
else
txt="ERROR: failed to create the mesh '"+visual.meshFilename+"' or '"+visual.meshFilename_alt+"' with extension type "+boost::lexical_cast<std::string>(visual.meshExtension);
printToConsole(txt.c_str());
}
else
visual.n = scaleShapeIfRequired(visual.n,visual.mesh_scaling);
}
else if (!isArrayEmpty(visual.sphere_size))
visual.n = simCreatePureShape( 1,1+2+16, visual.sphere_size, mass, NULL);
else if (!isArrayEmpty(visual.cylinder_size))
visual.n = simCreatePureShape( 2,1+2+16, visual.cylinder_size, mass, NULL);
else if (!isArrayEmpty(visual.box_size))
visual.n = simCreatePureShape( 0,1+2+16, visual.box_size, mass, NULL);
}
//collisions.clear();
//mass=0.1;
//collision
for (int i=0; i<collisions.size(); i++) {
urdfElement &collision = collisions[i];
if(!collision.meshFilename.empty())
{
std::string fname(collision.meshFilename);
bool exists=true;
bool useAlt=false;
if (!simDoesFileExist(fname.c_str()))
{
fname=collision.meshFilename_alt;
exists=simDoesFileExist(fname.c_str());
useAlt=true;
}
if (!exists)
printToConsole("ERROR: the mesh file could not be found");
else
collision.n = simImportShape(collision.meshExtension,fname.c_str(),0,0.0001f,1.0);
if (collision.n == -1)
{
if (!useAlt)
txt="ERROR: failed to create the mesh '"+collision.meshFilename+"' with extension type "+boost::lexical_cast<std::string>(collision.meshExtension);
else
txt="ERROR: failed to create the mesh '"+collision.meshFilename+"' or '"+collision.meshFilename_alt+"' with extension type "+boost::lexical_cast<std::string>(collision.meshExtension);
printToConsole(txt.c_str());
}
else
{
collision.n=scaleShapeIfRequired(collision.n,collision.mesh_scaling);
if (createVisualIfNone&&(visuals.size()==0))
{ // We create a visual from the collision shape (before it gets morphed hereafter):
simRemoveObjectFromSelection(sim_handle_all,-1);
simAddObjectToSelection(sim_handle_single,collision.n);
simCopyPasteSelectedObjects();
addVisual();
currentVisual().n = simGetObjectLastSelection();
}
int p;
int convInts[5]={1,500,200,0,0}; // 3rd value from 100 to 500 on 5/2/2014
float convFloats[5]={100.0f,30.0f,0.25f,0.0f,0.0f};
if ( convexDecomposeNonConvexCollidables&&(simGetObjectIntParameter(collision.n,3017,&p)>0)&&(p==0) )
{
int aux=1+4+8+16+64;
if (showConvexDecompositionDlg)
aux=1+2+8+16+64;
showConvexDecompositionDlg=false;
simConvexDecompose(collision.n,aux,convInts,convFloats); // we generate convex shapes!
}
simSetObjectIntParameter(collision.n,3003,!inertiaPresent); // we make it non-static if there is an inertia
simSetObjectIntParameter(collision.n,3004,1); // we make it respondable since it is a collision object
}
}
else if (!isArrayEmpty(collision.sphere_size))
collision.n = simCreatePureShape( 1,1+2+4+8+16*(!inertiaPresent), collision.sphere_size, mass, NULL);
else if (!isArrayEmpty(collision.cylinder_size))
collision.n = simCreatePureShape( 2,1+2+4+8+16*(!inertiaPresent), collision.cylinder_size, mass, NULL);
else if (!isArrayEmpty(collision.box_size))
collision.n = simCreatePureShape( 0,1+2+4+8+16*(!inertiaPresent), collision.box_size, mass, NULL);
}
// Hack to draw COM in the collision layer
/*
addCollision();
currentCollision().xyz[0] = inertial_xyz[0];
currentCollision().xyz[1] = inertial_xyz[1];
currentCollision().xyz[0] = inertial_xyz[2];
currentCollision().rpy[0] = 1.5;
float dummySize[3]={0.01f,0.01f,0.01f};
currentCollision().n = simCreatePureShape( 1,1+2+16, dummySize, mass, NULL);
*/
// Grouping collisions shapes
nLinkCollision = groupShapes(collisions);
// Inertia
if (inertiaPresent)
{
C3Vector euler;
if (nLinkCollision==-1)
{ // we do not have a collision object. Let's create a dummy collision object, since inertias can't exist on their own in V-REP:
float dummySize[3]={0.01f,0.01f,0.01f};
//nLinkCollision = simCreatePureShape( 1,1+2+4, dummySize, mass, NULL); // we make it non-respondable!
nLinkCollision = simCreatePureShape( 1,1+2+16, dummySize, mass, NULL);
}
C7Vector inertiaFrame;
inertiaFrame.X.set(inertial_xyz);
inertiaFrame.Q=getQuaternionFromRpy(inertial_rpy);
//simSetObjectPosition(nLinkCollision,-1,inertiaFrame.X.data);
//C7Vector collisionFrame;
//collisionFrame.X.set(collision_xyz);
//collisionFrame.Q=getQuaternionFromRpy(collision_rpy);
C7Vector collisionFrame;
simGetObjectPosition(nLinkCollision,-1,collisionFrame.X.data);
simGetObjectOrientation(nLinkCollision,-1,euler.data);
collisionFrame.Q.setEulerAngles(euler);
//C4X4Matrix x((collisionFrame.getInverse()*inertiaFrame).getMatrix());
C4X4Matrix x(inertiaFrame.getMatrix());
float i[12]={x.M(0,0),x.M(0,1),x.M(0,2),x.X(0),x.M(1,0),x.M(1,1),x.M(1,2),x.X(1),x.M(2,0),x.M(2,1),x.M(2,2),x.X(2)};
simSetShapeMassAndInertia(nLinkCollision,mass,inertia,C3Vector::zeroVector.data,i);
//std::cout << "Mass: " << mass << std::endl;
}
else
{
if (nLinkCollision!=-1)
{
std::string txt("ERROR: found a collision object without inertia data for link '"+ name+"'. Is that link meant to be static?");
printToConsole(txt.c_str());
}
}
if (createVisualIfNone&&(visuals.size()==0)&&(nLinkCollision!=-1))
{ // We create a visual from the collision shape (meshes were handled earlier):
addVisual();
urdfElement &visual = currentVisual();
simRemoveObjectFromSelection(sim_handle_all,-1);
simAddObjectToSelection(sim_handle_single,nLinkCollision);
simCopyPasteSelectedObjects();
visual.n=simGetObjectLastSelection();
simSetObjectIntParameter(visual.n,3003,1); // we make it static since only visual
simSetObjectIntParameter(visual.n,3004,0); // we make it non-respondable since only visual
}
// Set the respondable mask:
if (nLinkCollision!=-1)
simSetObjectIntParameter(nLinkCollision,3019,0xff00); // colliding with everything except with other objects in that tree hierarchy
// Grouping shapes
nLinkVisual = groupShapes(visuals);
// Set the names, visibility, etc.:
if (nLinkVisual!=-1)
{
setVrepObjectName(nLinkVisual,std::string(name+"_visual").c_str());
const float specularDiffuse[3]={0.3f,0.3f,0.3f};
if (nLinkCollision!=-1)
{ // if we have a collision object, we attach the visual object to it, then forget the visual object
C7Vector collisionFrame;
C3Vector euler;
simGetObjectPosition(nLinkCollision,-1,collisionFrame.X.data);
simGetObjectOrientation(nLinkCollision,-1,euler.data);
collisionFrame.Q.setEulerAngles(euler);
C7Vector visualFrame;
simGetObjectPosition(nLinkVisual,-1,visualFrame.X.data);
simGetObjectOrientation(nLinkVisual,-1,euler.data);
visualFrame.Q.setEulerAngles(euler);
C7Vector x(collisionFrame.getInverse()*visualFrame);
simSetObjectPosition(nLinkVisual,-1,x.X.data);
simSetObjectOrientation(nLinkVisual,-1,x.Q.getEulerAngles().data);
simSetObjectParent(nLinkVisual,nLinkCollision,0);
}
}
if (nLinkCollision!=-1)
{
setVrepObjectName(nLinkCollision,std::string(name+"_respondable").c_str());
if (hideCollisionLinks)
simSetObjectIntParameter(nLinkCollision,10,256); // we "hide" that object in layer 9
}
}
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);
}
}
