bool SrhSyntouchController::init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name)
{
ROS_DEBUG(" --------- ");
ROS_DEBUG_STREAM("Init: " << joint_name);
assert(robot);
robot_ = robot;
last_time_ = robot->getTime();
joint_state_ = robot_->getJointState(joint_name);
if (!joint_state_)
{
ROS_ERROR("SrhMixedPositionVelocityController could not find joint named \"%s\"\n",
joint_name.c_str());
return false;
}
if (!joint_state_->calibrated_)
{
ROS_ERROR("Joint %s not calibrated for SrhSyntouchController", joint_name.c_str());
return false;
}
//init the pointer to the biotacs data, updated at 1kHz
actuator_ = static_cast<sr_actuator::SrActuator*>( robot->model_->getActuator( joint_name ) );
after_init();
return true;
}
bool SrhExampleController::init(pr2_mechanism_model::RobotState *robot, const std::string &joint_name)
{
assert(robot);
robot_ = robot;
last_time_ = robot->getTime();
//We need to store 2 different joint states for the joint 0s:
// They control the distal and the middle joint with the same control.
if( joint_name.substr(3,1).compare("0") == 0)
{
has_j2 = true;
//The joint 0 is name *FJ0, and is controlling *J1 + *J2.
std::string j1 = joint_name.substr(0,3) + "1";
std::string j2 = joint_name.substr(0,3) + "2";
//Get the pointer to the joint state for *J1
joint_state_ = robot_->getJointState(j1);
if (!joint_state_)
{
ROS_ERROR("SrhMixedPositionVelocityController could not find joint named \"%s\"\n",
joint_name.c_str());
return false;
}
//Get the pointer to the joint state for *J2
joint_state_2 = robot_->getJointState(j2);
if (!joint_state_2)
{
ROS_ERROR("SrhMixedPositionVelocityController could not find joint named \"%s\"\n",
joint_name.c_str());
return false;
}
}
else //"normal" joints: one controller controls one joint
{
has_j2 = false;
//get the pointer to the joint state
joint_state_ = robot_->getJointState(joint_name);
if (!joint_state_)
{
ROS_ERROR("SrhMixedPositionVelocityController could not find joint named \"%s\"\n",
joint_name.c_str());
return false;
}
}
// after init creates the subscriber to the /command topic
after_init();
return true;
}
// CHECK-LABEL: define void @_ZN7pr285954testEv()
void test() {
// CHECK: %[[SRC:.*]] = alloca [3 x [5 x %[[A:.*]]]], align 1
A array[3][5];
// Skip over the initialization loop.
// CHECK: call {{.*}}after_init
after_init();
// CHECK: %[[DST_0:.*]] = getelementptr {{.*}} [3 x [5 x %[[A]]]]* %[[DST:.*]], i64 0, i64 0
// CHECK: br label
// CHECK: %[[I:.*]] = phi i64 [ 0, %{{.*}} ], [ %[[I_NEXT:.*]], {{.*}} ]
// CHECK: %[[DST_I:.*]] = getelementptr {{.*}} [5 x %[[A]]]* %[[DST_0]], i64 %[[I]]
// CHECK: %[[SRC_I:.*]] = getelementptr {{.*}} [3 x [5 x %[[A]]]]* %[[SRC]], i64 0, i64 %[[I]]
//
// CHECK: %[[DST_I_0:.*]] = getelementptr {{.*}} [5 x %[[A]]]* %[[DST_I]], i64 0, i64 0
// CHECK: br label
// CHECK: %[[J:.*]] = phi i64 [ 0, %{{.*}} ], [ %[[J_NEXT:.*]], {{.*}} ]
// CHECK: %[[DST_I_J:.*]] = getelementptr {{.*}} %[[A]]* %[[DST_I_0]], i64 %[[J]]
// CHECK: %[[DST_0_0:.*]] = bitcast [5 x %[[A]]]* %[[DST_0]] to %[[A]]*
// CHECK: %[[SRC_I_J:.*]] = getelementptr {{.*}} [5 x %[[A]]]* %[[SRC_I]], i64 0, i64 %[[J]]
//
// CHECK: invoke void @_ZN7pr285954TempC1Ev
// CHECK: invoke void @_ZN7pr285951AC1ERKS0_RKNS_4TempE
// CHECK: invoke void @_ZN7pr285954TempD1Ev
//
// CHECK: add nuw i64 %[[J]], 1
// CHECK: icmp eq
// CHECK: br i1
//
// CHECK: add nuw i64 %[[I]], 1
// CHECK: icmp eq
// CHECK: br i1
//
// CHECK: ret void
//
// CHECK: landingpad
// CHECK: landingpad
// CHECK: br label %[[CLEANUP:.*]]{{$}}
// CHECK: landingpad
// CHECK: invoke void @_ZN7pr285954TempD1Ev
// CHECK: br label %[[CLEANUP]]
//
// CHECK: [[CLEANUP]]:
// CHECK: icmp eq %[[A]]* %[[DST_0_0]], %[[DST_I_J]]
// CHECK: %[[T0:.*]] = phi %[[A]]*
// CHECK: %[[T1:.*]] = getelementptr inbounds %[[A]], %[[A]]* %[[T0]], i64 -1
// CHECK: call void @_ZN7pr285951AD1Ev(%[[A]]* %[[T1]])
// CHECK: icmp eq %[[A]]* %[[T1]], %[[DST_0_0]]
(void) [array]{};
}
