void CSubscriberData::setJointForceCallback(const std_msgs::Float64::ConstPtr& force)
{
if (_handleGeneralCallback_before())
{
if (simSetJointForce(auxInt1,(float)force->data)==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setJointStateCallback(const vrep_common::JointSetStateData::ConstPtr& data)
{
if ( (data->handles.data.size()>0)&&(data->handles.data.size()==data->setModes.data.size())&&(data->handles.data.size()==data->values.data.size()) )
{
for (unsigned int i=0;i<data->handles.data.size();i++)
{
int handle=data->handles.data[i];
unsigned char setMode=data->setModes.data[i];
float val=data->values.data[i];
if (setMode==0)
simSetJointPosition(handle,val);
if (setMode==1)
simSetJointTargetPosition(handle,val);
if (setMode==2)
simSetJointTargetVelocity(handle,val);
if (setMode==3)
simSetJointForce(handle,val);
}
}
}
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);
}
}
}
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);
}
}