void loop()
{
joint_1_position = jointGet(joint_1_address, I2C_COMMAND_JOINT_GET_POSITION);
// Report it to the publisher.
joint_1_position_msg.data = joint_1_position;
joint_1_position_publisher.publish(&joint_1_position_msg);
// Now, check if we have a new setpoint from the listener.
if (joint_1_new_setpoint != joint_1_current_setpoint)
{
jointSet(joint_1_address,
I2C_COMMAND_JOINT_SET_SETPOINT,
joint_1_new_setpoint);
joint_1_current_setpoint = joint_1_new_setpoint;
}
joint_2_position = jointGet(joint_2_address, I2C_COMMAND_JOINT_GET_POSITION);
// Report it to the publisher.
joint_2_position_msg.data = joint_2_position;
joint_2_position_publisher.publish(&joint_2_position_msg);
// Now, check if we have a new setpoint from the listener.
if (joint_2_new_setpoint != joint_2_current_setpoint)
{
jointSet(joint_2_address,
I2C_COMMAND_JOINT_SET_SETPOINT,
joint_2_new_setpoint);
joint_2_current_setpoint = joint_2_new_setpoint;
}
joint_3_position = jointGet(joint_3_address, I2C_COMMAND_JOINT_GET_POSITION);
// Report it to the publisher.
joint_3_position_msg.data = joint_3_position;
joint_3_position_publisher.publish(&joint_3_position_msg);
// Now, check if we have a new setpoint from the listener.
if (joint_3_new_setpoint != joint_3_current_setpoint)
{
jointSet(joint_3_address,
I2C_COMMAND_JOINT_SET_SETPOINT,
joint_3_new_setpoint);
joint_3_current_setpoint = joint_3_new_setpoint;
}
nh.spinOnce();
delay(100);
}
void mtsIntuitiveResearchKitPSM::RunPositionCartesian(void)
{
//! \todo: should prevent user to go to close to RCM!
if (IsCartesianGoalSet == true) {
vctDoubleVec jointSet(6, 0.0);
jointSet.Assign(JointCurrent, 6);
// compute desired slave position
CartesianPositionFrm.From(CartesianGoalSet.Goal());
CartesianPositionFrm = CartesianPositionFrm * Frame6to7Inverse;
Manipulator.InverseKinematics(jointSet, CartesianPositionFrm);
jointSet.resize(7);
jointSet[6] = DesiredOpenAngle;
#if 1 // Anton
const double difference = JointCurrent[3] - jointSet[3];
const double differenceInTurns = nearbyint(difference / (2.0 * cmnPI));
jointSet[3] = jointSet[3] + differenceInTurns * 2.0 * cmnPI;
/*
if (differenceInTurns != 0.0) {
CMN_LOG_CLASS_RUN_DEBUG << GetName()
<< " diff = " << difference
<< " turns = " << difference / (2.0 * cmnPI)
<< " corr = " << differenceInTurns
<< " res = " << jointSet[3] << std::endl;
}
*/
#endif // Anton
SetPositionJointLocal(jointSet);
// reset flag
IsCartesianGoalSet = false;
}
}
void mtsIntuitiveResearchKitArm::SetPositionGoalCartesian(const prmPositionCartesianSet & newPosition)
{
if (CurrentStateIs(mtsIntuitiveResearchKitArmTypes::DVRK_POSITION_GOAL_CARTESIAN)) {
const size_t nbJoints = this->NumberOfJoints();
const size_t nbJointsKin = this->NumberOfJointsKinematics();
const double currentTime = this->StateTable.GetTic();
// copy current position
vctDoubleVec jointSet(JointGet.Ref(nbJointsKin));
// compute desired slave position
CartesianPositionFrm.From(newPosition.Goal());
if (this->InverseKinematics(jointSet, BaseFrame.Inverse() * CartesianPositionFrm) == robManipulator::ESUCCESS) {
// resize to proper number of joints
jointSet.resize(nbJoints);
// keep values of joints not handled by kinematics (PSM jaw)
if (nbJoints > nbJointsKin) {
// check if there is a trajectory active
if (currentTime >= JointTrajectory.EndTime) {
// past any trajectory, use last desired joint position
jointSet.Ref(nbJoints - nbJointsKin,
nbJointsKin).Assign(JointGetDesired.Ref(nbJoints - nbJointsKin,
nbJointsKin));
} else {
// there is an ongoing trajectory, use the trajectory goal
jointSet.Ref(nbJoints - nbJointsKin,
nbJointsKin).Assign(JointTrajectory.Goal.Ref(nbJoints - nbJointsKin,
nbJointsKin));
}
}
const double currentTime = this->StateTable.GetTic();
// starting point is last requested to PID component
JointTrajectory.Start.Assign(JointGetDesired, NumberOfJoints());
JointTrajectory.Goal.Assign(jointSet);
JointTrajectory.LSPB.Set(JointTrajectory.Start, JointTrajectory.Goal,
JointTrajectory.Velocity, JointTrajectory.Acceleration,
currentTime, robLSPB::LSPB_DURATION);
JointTrajectory.EndTime = currentTime + JointTrajectory.LSPB.Duration();
} else {
MessageEvents.Error(this->GetName() + " unable to solve inverse kinematics.");
JointTrajectory.GoalReachedEvent(false);
}
}
}
void mtsIntuitiveResearchKitArm::RunPositionCartesian(void)
{
if (IsGoalSet) {
// copy current position
vctDoubleVec jointSet(JointGet.Ref(NumberOfJointsKinematics()));
// compute desired slave position
CartesianPositionFrm.From(CartesianSetParam.Goal());
if (this->InverseKinematics(jointSet, BaseFrame.Inverse() * CartesianPositionFrm) == robManipulator::ESUCCESS) {
// assign to joints used for kinematics
JointSet.Ref(NumberOfJointsKinematics()).Assign(jointSet);
// finally send new joint values
SetPositionJointLocal(JointSet);
} else {
MessageEvents.Error(this->GetName() + " unable to solve inverse kinematics.");
}
// reset flag
IsGoalSet = false;
}
}
