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):
"""
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):
"""
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 publish_state_and_config(self, data, Xfilt, Xref):
xmsg = PlanarSystemState()
qmsg = PlanarSystemConfig()
xmsg.header = data.header
qmsg.header = data.header
tools.array_to_state(self.system, Xfilt, xmsg)
tools.array_to_config(self.system, Xfilt[0:self.system.nQ], qmsg)
self.filt_state_pub.publish(xmsg)
self.filt_pub.publish(qmsg)
# publish ref data:
xmsg = PlanarSystemState()
qmsg = PlanarSystemConfig()
xmsg.header = data.header
qmsg.header = data.header
tools.array_to_state(self.system, Xref, xmsg)
tools.array_to_config(self.system, Xref[0:self.system.nQ], qmsg)
self.ref_state_pub.publish(xmsg)
self.ref_pub.publish(qmsg)
return
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):
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 publish_state_and_config(self, data, Xfilt, Xref):
xmsg = PlanarSystemState()
qmsg = PlanarSystemConfig()
xmsg.header = data.header
qmsg.header = data.header
tools.array_to_state(self.system, Xfilt, xmsg)
tools.array_to_config(self.system, Xfilt[0:self.system.nQ], qmsg)
self.filt_state_pub.publish(xmsg)
self.filt_pub.publish(qmsg)
# publish ref data:
xmsg = PlanarSystemState()
qmsg = PlanarSystemConfig()
xmsg.header = data.header
qmsg.header = data.header
tools.array_to_state(self.system, Xref, xmsg)
tools.array_to_config(self.system, Xref[0:self.system.nQ], qmsg)
self.ref_state_pub.publish(xmsg)
self.ref_pub.publish(qmsg)
return
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
