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]{}; }