def __init__(self, agent_name):
self.agent_name = agent_name
rospy.init_node(agent_name)
self._rate = rospy.Rate(10)
Model.__init__(self)
# Visual.__init__(self, agent_name)
self.ctrllr = DErgControl(agent_name, Model(), num_basis=10)
self.__br = tf.TransformBroadcaster()
# TODO: consider moving this into the target dist class
self._target_dist_pub = rospy.Publisher(agent_name + '/target_dist',
GridMap,
queue_size=1)
gridmap = GridMap()
arr = Float32MultiArray()
print(np.shape(self.ctrllr._targ_dist.grid_vals[::-1]))
arr.data = self.ctrllr._targ_dist.grid_vals[::-1]
arr.layout.dim.append(MultiArrayDimension())
arr.layout.dim.append(MultiArrayDimension())
arr.layout.dim[0].label = "column_index"
arr.layout.dim[0].size = 50
arr.layout.dim[0].stride = 50 * 50
arr.layout.dim[1].label = "row_index"
arr.layout.dim[1].size = 50
arr.layout.dim[1].stride = 50
gridmap.layers.append("elevation")
gridmap.data.append(arr)
gridmap.info.length_x = 10
gridmap.info.length_y = 10
gridmap.info.pose.position.x = 5
gridmap.info.pose.position.y = 5
gridmap.info.header.frame_id = "world"
gridmap.info.resolution = 0.2
self._grid_msg = gridmap
# Simulate publisher for encountering dd or ee
self.dd_pub = rospy.Publisher('/dd_loc', Pose, queue_size=1)
self.ee_pub = rospy.Publisher('/ee_loc', Pose, queue_size=1)
self.pose_msg = Pose()
self.pose_msg.position.z = 0.
self.pose_msg.orientation.x = 0.0
self.pose_msg.orientation.x = 0.0
self.pose_msg.orientation.x = 0.0
self.pose_msg.orientation.w = 1.0
def __init__(self):
threading.Thread.__init__(self)
self.clock_sub_name = 'clock'
self.hyq_wbs_sub_name = "/hyq/robot_states"
self.gridMapTopicName = "/local_gridmap"
self.hyq_wbs = dict()
self.hyq_rcf_debug = GridMap()
self.actuation_polygon_topic_name = "/hyq/actuation_polygon"
self.support_region_topic_name = "/hyq/support_region"
self.force_polygons_topic_name = "/hyq/force_polygons"
self.sim_time = 0.0
def talker():
gridmap = GridMap()
math = Math()
p = HyQSim()
p.start()
p.register_node()
name = "Actuation_region"
point = Point()
polygonVertex = Point()
actPolygon = Polygon3D()
converter = gridMapConverter()
i = 0
for j in range(0, 50):
vertices1 = [point]
actPolygons = [polygonVertex]
# poly = []
# print("Time: " + str(i*0.004) + "s and Simulation time: " + str(p.get_sim_time()/60))
p.get_sim_wbs()
converter.getParamsFromRosDebugTopic(p.hyq_rcf_debug)
trunk_mass = 85.
mu = 0.8
ng = 4
axisZ = np.array([[0.0], [0.0], [1.0]])
''' normals '''
n1 = np.transpose(
np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ))
n2 = np.transpose(
np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ))
n3 = np.transpose(
np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ))
n4 = np.transpose(
np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ))
normals = np.vstack([n1, n2, n3, n4])
print 'contacts: ', 1
time.sleep(1.0 / 25.0)
i += 1
print 'de registering...'
p.deregister_node()
def convert(df, resolution, **kwargs):
"""
Converts a DataFrame to GridMap
Convention of the GridMap is described in:
https://github.com/ANYbotics/grid_map/raw/master/grid_map_core/doc/grid_map_conventions.pdf
^
|
|
|
length_x |
|
|
length_y |
<-----------------+
:param df:
:type df: pd.DataFrame
:param resolution:
:return:
"""
frame_id = kwargs['frame_id'] if 'frame_id' in kwargs else 'world'
layer = kwargs['layer'] if 'layer' in kwargs else 'elevation'
origin = kwargs['origin'] if 'origin' in kwargs else None
grids_x = df['x'].nunique()
grids_y = df['y'].nunique()
df = df.sort_values(['y', 'x'], ascending=False)
gm = GridMap()
gm.info.header.frame_id = frame_id
if 'stamp' in kwargs and kwargs['stamp'] != 'now':
gm.info.header.stamp = kwargs['stamp']
else:
gm.info.header.stamp = rospy.Time.now()
gm.info.resolution = resolution
gm.info.length_x = grids_x * resolution
gm.info.length_y = grids_y * resolution
if origin is None:
# Put origin at the center
pose = Pose()
pose.position.x = 0
pose.position.y = 0
pose.orientation.w = 1
elif type(origin) is list or type(origin) is tuple:
pose = Pose()
pose.position.x = origin[0]
pose.position.y = origin[1]
pose.orientation.w = 1
else:
pose = origin
gm.info.pose = pose
gm.layers.append(layer)
gm.basic_layers.append(layer)
# The dimensions
dim_col = MultiArrayDimension()
dim_col.label = 'column_index'
dim_col.size = grids_y
dim_col.stride = grids_x * grids_y
dim_row = MultiArrayDimension()
dim_row.label = 'row_index'
dim_row.size = grids_x
dim_row.stride = grids_x
# The actual data
flma = Float32MultiArray()
flma.layout.data_offset = 0
flma.layout.dim = [dim_col, dim_row]
flma.data = df['z'].values.tolist()
gm.data.append(flma)
return gm
#!/usr/bin/env python
import rospy
import numpy as np
from grid_map_msgs.msg import GridMap
from std_msgs.msg import Float32MultiArray, MultiArrayLayout, MultiArrayDimension
import tf
if __name__ == '__main__':
rospy.init_node('dist_test')
gridmap = GridMap()
x, y = np.meshgrid(np.linspace(0, 1), np.linspace(0, 1))
samples = np.c_[np.ravel(x, order="F"), np.ravel(y, order="F")]
grid_val = np.exp(-10. * np.sum((samples-np.array([0.2,0.2]))**2, axis=1))*10 \
+ np.exp(-10. * np.sum((samples-np.array([0.8,0.5]))**2, axis=1))*10
arr = Float32MultiArray()
arr.data = grid_val[::-1]
arr.layout.dim.append(MultiArrayDimension())
arr.layout.dim.append(MultiArrayDimension())
arr.layout.dim[0].label = "column_index"
arr.layout.dim[0].size = 50
arr.layout.dim[0].stride = 50 * 50
arr.layout.dim[1].label = "row_index"
arr.layout.dim[1].size = 50
arr.layout.dim[1].stride = 50