void KeySigEvent::setAccidentalType(int v)
{
_accidentalType = v;
_custom = false;
_invalid = false;
enforceLimits();
}
KeySigEvent::KeySigEvent(int n)
{
_accidentalType = n;
_naturalType = 0;
_customType = 0;
_custom = false;
_invalid = false;
enforceLimits();
}
void write(ros::Time time, ros::Duration period)
{
enforceLimits(period);
switch (getControlStrategy())
{
case JOINT_POSITION:
for(int j=0; j < n_joints_; j++)
{
// according to the gazebo_ros_control plugin, this must *not* be called if SetForce is going to be called
// but should be called when SetPostion is going to be called
// so enable this when I find the SetMaxForce reset.
// sim_joints_[j]->SetMaxForce(0, joint_effort_limits_[j]);
#if GAZEBO_MAJOR_VERSION >= 4
sim_joints_[j]->SetPosition(0, joint_position_command_[j]);
#else
sim_joints_[j]->SetAngle(0, joint_position_command_[j]);
#endif
}
break;
case CARTESIAN_IMPEDANCE:
ROS_WARN("CARTESIAN IMPEDANCE NOT IMPLEMENTED");
break;
case JOINT_IMPEDANCE:
// compute the gracity term
f_dyn_solver_->JntToGravity(joint_position_kdl_, gravity_effort_);
for(int j=0; j < n_joints_; j++)
{
// replicate the joint impedance control strategy
// tau = k (q_FRI - q_msr) + tau_FRI + D(q_msr) + f_dyn(q_msr)
const double stiffness_effort = 0.0;//10.0*( joint_position_command_[j] - joint_position_[j] ); // joint_stiffness_command_[j]*( joint_position_command_[j] - joint_position_[j] );
//double damping_effort = joint_damping_command_[j]*( joint_velocity_[j] );
const double effort = stiffness_effort + joint_effort_command_[j] + gravity_effort_(j);
sim_joints_[j]->SetForce(0, effort);
}
break;
case GRAVITY_COMPENSATION:
ROS_WARN("CARTESIAN IMPEDANCE NOT IMPLEMENTED");
break;
}
}
void KeySigEvent::initFromSubtype(int st)
{
union U {
int subtype;
struct {
int _accidentalType:4;
int _naturalType:4;
unsigned _customType:16;
bool _custom : 1;
bool _invalid : 1;
};
};
U a;
a.subtype = st;
_accidentalType = a._accidentalType;
_customType = a._customType;
_custom = a._custom;
_invalid = a._invalid;
enforceLimits();
}
void KeySigEvent::initFromSubtype(int st)
{
union U {
int subtype;
struct {
int _key:4;
int _naturalType:4;
unsigned _customType:16;
bool _custom : 1;
bool _invalid : 1;
};
};
U a;
a.subtype = st;
_key = Key(a._key);
// _customType = a._customType;
_custom = a._custom;
if (a._invalid)
_key = Key::INVALID;
enforceLimits();
}
void KeySigEvent::setKey(Key v)
{
_key = v;
_custom = false;
enforceLimits();
}
