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);
This file is responsible for the initialisation of the world parameters that are controlled by ODE.
Convenience class for mapping from physical ODE joints in the model to control joints in the ICUB spe...
OdeLogicalJoint * at(int index)
Access a nested control unit.
void controlModeChanged(int cm)
void init(const char *unit, const char *type, int index, int sign, RobotConfig &conf)
Initialize a regular control unit.
virtual ~OdeLogicalJoint()
Destructor.
void setPosition(double target)
Drive towards an angle setpoint (in ICUB units/sign).
OdeLogicalJoint * nest(int len)
Create an array of nested control units.
double getAngleRaw()
Get the angle of an associated joint in unconverted units and sign.
double getVelocityRaw()
Get the velocity of an associated joint in unconverted units and sign.
double getTorque()
this is just a fake torque interface for now
void setVelocityRaw(double target)
Set raw velocity of joint (in ODE units/sign).
OdeLogicalJoint()
Constructor.
void setVelocity(double target)
Set velocity of joint (in ICUB units/sign).
void setControlParameters(double vel, double acc)
Set velocity and acceleration control parameters.
void setTorque(double target)
Set the reference torque.
double getSpeedSetpoint()
Get the current target velocity.
virtual double getMotorMaxTorque()=0
virtual double getMotorDryFriction()=0