26 #include <yarp/os/LogStream.h>
75 yDebug(
"Vergence motor remap %d\n", sgn);
97 yDebug(
"Motor %s %s %d %d\n", unit, type, index, sign);
101 }
else if (stype==
"universalAngle1") {
104 }
else if (stype==
"universalAngle2") {
108 yError(
"Unknown axis type %s\n", type);
111 if (sunit==
"leftarm") {
115 }
else if (universal==2) {
121 }
else if (sunit==
"rightarm") {
125 }
else if (universal==2) {
132 else if (sunit==
"head") {
137 else if (sunit==
"leftleg") {
144 else if (sunit==
"rightleg") {
151 else if (sunit==
"torso") {
159 yError(
"Unknown body unit %s\n", unit);
162 feedback =
new dJointFeedback;
163 dJointSetFeedback(*joint, feedback);
165 dJointGetHingeAxis(*joint, axis);
166 else if(universal==1)
167 dJointGetUniversalAxis1(*joint, axis);
168 else if(universal==2)
169 dJointGetUniversalAxis2(*joint, axis);
177 if(!hinged || !feedback)
189 return dJointGetHingeAngle(*joint);
190 }
else if (universal == 1) {
191 return dJointGetUniversalAngle1(*joint);
192 }
else if (universal == 2) {
193 return dJointGetUniversalAngle2(*joint);
208 return dJointGetHingeAngleRate(*joint);
209 }
else if (universal == 1) {
210 return dJointGetUniversalAngle1Rate(*joint);
211 }
else if (universal == 2) {
212 return dJointGetUniversalAngle2Rate(*joint);
225 for (
int i=0; i<subLength; i++) {
241 for (
int i=0; i<subLength; i++) {
249 for (
int i=0; i<subLength; i++) {
258 speedSetpoint = target;
261 (*speed) = speedSetpoint;
264 if (fabs(target)>0.1) {
280 dJointSetHingeParam(*joint, dParamFMax, dryFriction);
281 dJointSetHingeParam(*joint, dParamVel, 0.0);
284 dJointSetHingeParam(*joint, dParamFMax, maxTorque);