def ref_config_service_handler(self, req):
     """
     Return a reference configuration for the system by either
     index or time
     """
     if req.t != 0:
         # then use time, otherwise use the index
         try:
             index = [i for i,x in enumerate(self.tref) \
                      if x <= req.t < self.tref[i+1]][0]
         except:
             rospy.logerr("Invalid time for service request!")
             return None
         if index > len(self.tref)-1:
             rospy.logerr("Requested index is too large!")
             return None
         config = PlanarSystemConfig()
         config.xm = self.Qref[index][0]
         config.ym = self.Qref[index][1]
         config.xr = self.Qref[index][2]
         config.r  = self.Qref[index][3]
         return {'config': config}
     else:
         # use the index:
         if req.index > len(self.tref)-1:
             rospy.logerr("Requested index is too large!")
             return None
         else:
             config = PlanarSystemConfig()
             config.xm = self.Qref[req.index][0]
             config.ym = self.Qref[req.index][1]
             config.xr = self.Qref[req.index][2]
             config.r  = self.Qref[req.index][3]
             return {'config': config}
     return None
 def send_filt_and_ref(self, data):
     # publish the estimated pose:
     qest = PlanarSystemConfig()
     qest.header.stamp = data.header.stamp
     qest.header.frame_id = data.header.frame_id
     qest.xm = self.Xest2[0]
     qest.ym = self.Xest2[1]
     qest.xr = self.Xest2[2]
     qest.r  = self.Xest2[3]
     self.filt_pub.publish(qest)
     # publish estimated state
     xest = PlanarSystemState()
     xest.header.stamp = data.header.stamp
     xest.header.frame_id = data.header.frame_id
     xest.xm = self.Xest2[0]
     xest.ym = self.Xest2[1]
     xest.xr = self.Xest2[2]
     xest.r  = self.Xest2[3]
     xest.pxm = self.Xest2[4]
     xest.pym = self.Xest2[5]
     xest.vxr = self.Xest2[6]
     xest.vr  = self.Xest2[7]
     self.filt_state_pub.publish(xest)
     # publish the reference pose:
     qest = PlanarSystemConfig()
     qest.header.stamp = data.header.stamp
     qest.header.frame_id = data.header.frame_id
     qest.xm = self.Qref[self.k][0]
     qest.ym = self.Qref[self.k][1]
     qest.xr = self.Qref[self.k][2]
     qest.r  = self.Qref[self.k][3]
     self.ref_pub.publish(qest)
     return
 def ref_config_service_handler(self, req):
     if req.index is not 0:
         rospy.logerr("Current provider cannot handle index request")
         return None
     elif req.t < 0.0 or req.t > self.tf:
         rospy.logerr("Requested time %f is outside of valid horizon", req.t)
         return None
     # get X,U at requested time
     r = self.RM.calc_reference_traj(self.dsys, [req.t])
     if r:
         X,U = r
     else:
         rospy.logerr("calc_reference_traj returned None!")
         return None
     # fill out message
     config = PlanarSystemConfig()
     config.xm = X[0,self.system.get_config('xm').index]
     config.ym = X[0,self.system.get_config('ym').index]
     config.xr = X[0,self.system.get_config('xr').index]
     config.r = X[0,self.system.get_config('r').index]
     config.header.frame_id = "/optimization_frame"
     config.header.stamp = rospy.Time.now()
     time = req.t
     length = len(self.tvec)
     xtmp = X[0]
     dt = self.dt
     return {'config' : config,
             'state' : xtmp,
             'dt' : dt,
             'length' : length,
             'time' : time,
             }
 def ref_config_service_handler(self, req):
     if req.index is not 0:
         rospy.logerr("Current provider cannot handle index request")
         return None
     elif req.t < 0.0 or req.t > self.tf:
         rospy.logerr("Requested time %f is outside of valid horizon",
                      req.t)
         return None
     # get X,U at requested time
     r = self.RM.calc_reference_traj(self.dsys, [req.t])
     if r:
         X, U = r
     else:
         rospy.logerr("calc_reference_traj returned None!")
         return None
     # fill out message
     config = PlanarSystemConfig()
     config.xm = X[0, self.system.get_config('xm').index]
     config.ym = X[0, self.system.get_config('ym').index]
     config.xr = X[0, self.system.get_config('xr').index]
     config.r = X[0, self.system.get_config('r').index]
     config.header.frame_id = "/optimization_frame"
     config.header.stamp = rospy.Time.now()
     time = req.t
     length = len(self.tvec)
     xtmp = X[0]
     dt = self.dt
     return {
         'config': config,
         'state': xtmp,
         'dt': dt,
         'length': length,
         'time': time,
     }
 def send_filt_and_ref(self, data):
     # publish the estimated pose:
     qest = PlanarSystemConfig()
     qest.header.stamp = data.header.stamp
     qest.header.frame_id = data.header.frame_id
     qest.xm = self.Xest2[0]
     qest.ym = self.Xest2[1]
     qest.xr = self.Xest2[2]
     qest.r  = self.Xest2[3]
     self.filt_pub.publish(qest)
     # publish the reference pose:
     qest = PlanarSystemConfig()
     qest.header.stamp = data.header.stamp
     qest.header.frame_id = data.header.frame_id
     qest.xm = self.Qref[self.k][0]
     qest.ym = self.Qref[self.k][1]
     qest.xr = self.Qref[self.k][2]
     qest.r  = self.Qref[self.k][3]
     self.ref_pub.publish(qest)
     return
 def ref_config_service_handler(self, req):
     """
     Return a reference configuration for the system by either
     index or time
     """
     config = PlanarSystemConfig()
     if req.t != 0:
         # then use time, otherwise use the index
         try:
             index = [i for i,x in enumerate(self.tref) \
                      if x <= req.t < self.tref[i+1]][0]
         except:
             rospy.logerr("Invalid time for service request!")
             return None
         if index > len(self.tref)-1:
             rospy.logerr("Requested index is too large!")
             return None
     else:
         # use the index:
         if req.index > len(self.tref)-1:
             rospy.logerr("Requested index is too large!")
             return None
         index = req.index
     config.xm = self.Qref[index][0]
     config.ym = self.Qref[index][1]
     config.xr = self.Qref[index][2]
     config.r  = self.Qref[index][3]
     config.header.frame_id = "/optimization_frame"
     config.header.stamp = rospy.Time.now()
     time = self.tref[index]
     dt = self.dt
     length = len(self.tref)
     if index != len(self.tref)-1:
         utmp = self.Uref[index]
     else:
         utmp = self.Uref[-1]
     xtmp = self.Xref[index]
     return {'config' : config,
             'input' : utmp,
             'state' : xtmp,
             'dt' : dt,
             'length' : length,
             'time' : time,
             'index' : index}
    def inputcb(self, data):
        rospy.logdebug("inputcb triggered")

        # translation from base_link to left string expressed in the
        # base_link frame:
        vb = np.array([[0.0102, 0.0391, 0.086, 0]]).T
        gbs = np.array([[1, 0, 0, vb[0,0]],
                       [0, 0, -1, vb[1,0]],
                       [0, 1, 0, vb[2,0]],
                       [0, 0, 0, 1]])
        gsb = np.linalg.inv(gbs)

        # map to base stuff:
        pbm = np.array([[data.pose.pose.position.x,
                         data.pose.pose.position.y,
                         data.pose.pose.position.z]]).T
        qbm = np.array([[data.pose.pose.orientation.w,
                         data.pose.pose.orientation.x,
                         data.pose.pose.orientation.y,
                         data.pose.pose.orientation.z]]).T
        Rbm = quat_to_rot(qbm)
        gbm = to_se3(Rbm,pbm)
        gmb = np.linalg.inv(gbm)
        vm = np.dot(gmb,np.dot(gbs,np.dot(gsb,vb)))
        vm = np.ravel(vm)[0:3]

        ## so the first thing that we need to do is get the location
        ## of the robot in the "/optimization_frame"
        p = PointStamped()
        p.header = data.pose.header
        p.point = data.pose.pose.position
        p.point.x -= vm[0]
        p.point.y -= vm[1]
        p.point.z -= vm[2]
        quat = QuaternionStamped()
        quat.quaternion = data.pose.pose.orientation
        quat.header = p.header
        try:
            ptrans = self.listener.transformPoint("/optimization_frame", p)
            qtrans = self.listener.transformQuaternion("/optimization_frame", quat)
        except (tf.Exception):
            rospy.loginfo("tf exception caught !")
            return

        ## if we have not initialized the VI, let's do it, otherwise,
        ## let's integrate
        if not self.initialized_flag:
            q = [
                ptrans.point.x,
                ptrans.point.y-h0,
                ptrans.point.z,
                ptrans.point.x,
                ptrans.point.z,
                h0 ]
            self.sys.reset_integration(state=q)
            self.last_time = ptrans.header.stamp
            self.initialized_flag = True
            return

        # get operating_condition:
        if rospy.has_param("/operating_condition"):
            operating = rospy.get_param("/operating_condition")
        else:
            return


        # set string length
        self.len = data.left;

        ## if we are not running, just reset the parameter:
        if operating is not 2:
            self.initialized_flag = False
            return
        else:
            self.initialized_flag = True
            # if we are in the "running mode", let's integrate the VI,
            # and publish the results:
            rho = [ptrans.point.x, ptrans.point.z, self.len]
            dt = (ptrans.header.stamp - self.last_time).to_sec()
            self.last_time = ptrans.header.stamp
            rospy.logdebug("Taking a step! dt = "+str(dt))
            self.sys.take_step(dt, rho)
            ## now we can get the state of the system
            q = self.sys.get_current_configuration()

            ## now add the appropriate amount of noise:
            q += self.noise*np.random.normal(0,1,(self.sys.sys.nQ))
            new_point = PointStamped()
            new_point.point.x = q[0]
            new_point.point.y = q[1]
            new_point.point.z = q[2]
            new_point.header.frame_id = "/optimization_frame"
            new_point.header.stamp = rospy.rostime.get_rostime()
            rospy.logdebug("Publishing mass location")
            self.mass_pub.publish(new_point)

            ## we can also publish the planar results:
            config = PlanarSystemConfig()
            config.header.frame_id = "/optimization_frame"
            config.header.stamp = rospy.get_rostime()
            config.xm = q[0]
            config.ym = q[1]
            config.xr = rho[0]
            config.r = rho[2]
            self.plan_pub.publish(config)

            ## now we can send out the transform
            ns = rospy.get_namespace()
            fr = "mass_location"
            if len(ns) > 1:
                fr = rospy.names.canonicalize_name(ns+fr)

            # here we are going to do a bunch of geometry math to
            # ensure that the orientation of the frame that we are
            # placing at the mass location looks "nice"

            # zvec points from robot to the string
            zvec = np.array([q[0]-ptrans.point.x, q[1]-ptrans.point.y, q[2]-ptrans.point.z])
            zvec = zvec/np.linalg.norm(zvec)
            # get transform from incoming header frame to the optimization frame
            quat = qtrans.quaternion
            qtmp = np.array([quat.x, quat.y, quat.z, quat.w])
            # convert to SO(3), and extract the y-vector for the frame
            R1 = tf.transformations.quaternion_matrix(qtmp)[:3,:3]
            yvec = -R1[:,1]
            yvec[1] = (-yvec[0]*zvec[0]-yvec[2]*zvec[2])/zvec[1]
            # x vector is a cross of y and z
            xvec = np.cross(yvec, zvec)
            # build rotation matrix and send transform
            R = np.column_stack((xvec,yvec,zvec,np.array([0,0,0])))
            R = np.row_stack((R,np.array([0,0,0,1])))
            quat = tuple(tf.transformations.quaternion_from_matrix(R).tolist())
            self.br.sendTransform((new_point.point.x, new_point.point.y, new_point.point.z),
                                  quat,
                                  new_point.header.stamp,
                                  fr,
                                  "/optimization_frame")



        return