def make_tetra(node_indices, make_links=True, make_faces=True):
if len(node_indices) != 4:
rospy.logerr("bad node_indices length " + str(node_indices))
return None
tetra = Tetra()
tetra.node_indices = node_indices
links = []
if make_links:
for i in range(len(node_indices)):
for j in range(i + 1, len(node_indices)):
link = Link()
link.node_indices[0] = node_indices[i]
link.node_indices[1] = node_indices[j]
links.append(link)
faces = []
# 3
#
# 2
# 0 1
if make_faces:
face = Face()
face.node_indices = [node_indices[0], node_indices[2], node_indices[1]]
faces.append(face)
face = Face()
face.node_indices = [node_indices[0], node_indices[1], node_indices[3]]
faces.append(face)
face = Face()
face.node_indices = [node_indices[0], node_indices[3], node_indices[2]]
faces.append(face)
face = Face()
face.node_indices = [node_indices[2], node_indices[3], node_indices[1]]
faces.append(face)
return tetra, links, faces
def make_tetra(node_indices, make_links=True, make_faces=True):
if len(node_indices) != 4:
rospy.logerr("bad node_indices length " + str(node_indices))
return None
tetra = Tetra()
tetra.node_indices = node_indices
links = []
if make_links:
for i in range(len(node_indices)):
for j in range(i + 1, len(node_indices)):
link = Link()
link.node_indices[0] = node_indices[i]
link.node_indices[1] = node_indices[j]
links.append(link)
faces = []
# 3
#
# 2
# 0 1
if make_faces:
face = Face()
face.node_indices = [node_indices[0], node_indices[2], node_indices[1]]
faces.append(face)
face = Face()
face.node_indices = [node_indices[0], node_indices[1], node_indices[3]]
faces.append(face)
face = Face()
face.node_indices = [node_indices[0], node_indices[3], node_indices[2]]
faces.append(face)
face = Face()
face.node_indices = [node_indices[2], node_indices[3], node_indices[1]]
faces.append(face)
return tetra, links, faces
def make_tetra(node_indices, make_links=True):
tetra = Tetra()
tetra.node_indices = node_indices
links = []
if make_links:
for i in range(len(node_indices)):
for j in range(i + 1, len(node_indices)):
link = Link()
link.node_indices[0] = node_indices[i]
link.node_indices[1] = node_indices[j]
links.append(link)
return tetra, links
def make_soft_cube(name,
node_mass,
xs,
ys,
zs,
ln,
nx=4,
ny=4,
nz=4,
flip=1.0):
body = SoftBody()
body.config = make_soft_config()
body.name = name
# dynamic friction
body.config.kDF = 0.9
for i in range(nx):
for j in range(ny):
for k in range(nz):
n1 = Node()
n1.mass = node_mass
n1.position.x = xs + (i - nx / 2 + 0.5) * ln * flip
n1.position.y = ys + (j - ny / 2 + 0.5) * ln * flip
n1.position.z = zs + (k - nz / 2 + 0.5) * ln * flip
body.node.append(n1)
for ind1 in range(len(body.node)):
for i in range(2):
for j in range(2):
for k in range(2):
if i == 0 and j == 0 and k == 0:
continue
ind2 = ind1 + i * ny * nz + j * nz + k
if ind2 < len(body.node):
l1 = Link()
l1.node_indices[0] = ind1
l1.node_indices[1] = ind2
body.link.append(l1)
mat = Material()
mat.kLST = 0.25
mat.kVST = 0.1
mat.kAST = 0.1
body.material.append(mat)
return body
def make_soft_cube(name, node_mass,
xs, ys, zs,
ln,
nx=4, ny=4, nz=4, flip=1.0):
body = SoftBody()
body.config = make_soft_config()
body.name = name
# dynamic friction
body.config.kDF = 0.9
for i in range(nx):
for j in range(ny):
for k in range(nz):
n1 = Node()
n1.mass = node_mass
n1.position.x = xs + (i - nx/2 + 0.5) * ln * flip
n1.position.y = ys + (j - ny/2 + 0.5) * ln * flip
n1.position.z = zs + (k - nz/2 + 0.5) * ln * flip
body.node.append(n1)
for ind1 in range(len(body.node)):
for i in range(2):
for j in range(2):
for k in range(2):
if i == 0 and j == 0 and k == 0:
continue
ind2 = ind1 + i * ny * nz + j * nz + k
if ind2 < len(body.node):
l1 = Link()
l1.node_indices[0] = ind1
l1.node_indices[1] = ind2
body.link.append(l1)
mat = Material()
mat.kLST = 0.15 # 0.25
mat.kVST = 0.1
mat.kAST = 0.1
body.material.append(mat)
body.margin = nx * ln / 40.0
return body
def __init__(self):
rospy.wait_for_service('add_compound')
self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
add_compound_request = AddCompoundRequest()
add_compound_request.remove = rospy.get_param('~remove', False)
xs = rospy.get_param("~x", 0.0)
ys = rospy.get_param("~y", 0.0)
zs = rospy.get_param("~z", 1.0)
body = SoftBody()
body.name = "pyramid"
body.config = make_soft_config()
mass = 0.5
n1 = Node()
n1.mass = mass
n1.position.x = xs
n1.position.y = ys
n1.position.z = zs
body.node.append(n1)
n1 = Node()
n1.mass = mass
n1.position.x = xs + 1.0
n1.position.y = ys
n1.position.z = zs
body.node.append(n1)
n1 = Node()
n1.mass = mass
n1.position.x = xs
n1.position.y = ys + 1.0
n1.position.z = zs
body.node.append(n1)
n1 = Node()
n1.mass = mass
n1.position.x = xs
n1.position.y = ys
n1.position.z = zs + 1.0
body.node.append(n1)
l1 = Link()
l1.node_indices[0] = 0
l1.node_indices[1] = 1
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 0
l1.node_indices[1] = 2
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 0
l1.node_indices[1] = 3
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 1
l1.node_indices[1] = 2
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 1
l1.node_indices[1] = 3
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 2
l1.node_indices[1] = 3
body.link.append(l1)
mat = Material()
mat.kLST = 0.15
mat.kVST = 0.1
mat.kAST = 0.1
body.material.append(mat)
body.k_clusters = 8
body.margin = 0.05
add_compound_request.soft_body.append(body)
if False:
# make the top cylinder plate
top_plate = Body()
top_plate.name = "top_plate"
top_plate.mass = 0.3
rot90 = tf.transformations.quaternion_from_euler(
math.pi / 2.0, 0, 0)
radius = 1.5
thickness = 0.5
top_plate.pose.orientation.x = rot90[0]
top_plate.pose.orientation.y = rot90[1]
top_plate.pose.orientation.z = rot90[2]
top_plate.pose.orientation.w = rot90[3]
top_plate.pose.position.x = 0
top_plate.pose.position.y = 0
top_plate.pose.position.z = 1.7
top_plate.type = Body.CYLINDER
top_plate.scale.x = radius
top_plate.scale.y = thickness
top_plate.scale.z = radius
add_compound_request.body.append(top_plate)
rospy.loginfo(add_compound_request)
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)
def __init__(self):
self.pub = rospy.Publisher("marker", Marker, queue_size=2)
radius = rospy.get_param("~radius", 0.5)
levels = rospy.get_param("~levels", 3)
px = rospy.get_param("~x", 0.0)
name = rospy.get_param("~name", "ball")
body = SoftBody()
body.name = name
body.pose.position.x = px
body.pose.position.z = 0.2
body.pose.orientation.w = 1.0
# from http://blog.andreaskahler.com/2009/06/creating-icosphere-mesh-in-code.html
icost = (1.0 + math.sqrt(5.0)) / 2.0
tmp_radius = math.sqrt(1.0 + icost * icost)
scale = radius / tmp_radius
self.pts = []
self.subpts = {}
pt = Point()
self.pts.append(Point(-1, icost, 0))
self.pts.append(Point(1, icost, 0))
self.pts.append(Point(-1, -icost, 0))
self.pts.append(Point(1, -icost, 0))
self.pts.append(Point(0, -1, icost))
self.pts.append(Point(0, 1, icost))
self.pts.append(Point(0, -1, -icost))
self.pts.append(Point(0, 1, -icost))
self.pts.append(Point(icost, 0, -1))
self.pts.append(Point(icost, 0, 1))
self.pts.append(Point(-icost, 0, -1))
self.pts.append(Point(-icost, 0, 1))
# first construct an icosohedron
# rospy.loginfo(ico)
faces = []
faces.append([0, 11, 5])
faces.append([0, 5, 1])
faces.append([0, 1, 7])
faces.append([0, 7, 10])
faces.append([0, 10, 11])
if True:
faces.append([1, 5, 9])
faces.append([5, 11, 4])
faces.append([11, 10, 2])
faces.append([10, 7, 6])
faces.append([7, 1, 8])
faces.append([3, 9, 4])
faces.append([3, 4, 2])
faces.append([3, 2, 6])
faces.append([3, 6, 8])
faces.append([3, 8, 9])
faces.append([4, 9, 5])
faces.append([2, 4, 11])
faces.append([6, 2, 10])
faces.append([8, 6, 7])
faces.append([9, 8, 1])
for i in range(levels):
faces = self.subdivide(faces)
height = rospy.get_param("~height", radius * 1.1)
print len(self.pts)
# scale all points to be on sphere of desired radius
for i in range(len(self.pts)):
pt = self.pts[i]
scale = radius / math.sqrt(pt.x * pt.x + pt.y * pt.y + pt.z * pt.z)
pt.x *= scale
pt.y *= scale
pt.z *= scale
pt.z += height
self.pts[i] = pt
if False:
marker = Marker()
marker.header.frame_id = rospy.get_param("~frame_id", "map")
marker.ns = name
marker.id = 1
marker.type = Marker.TRIANGLE_LIST
marker.action = Marker.ADD
marker.pose.position.x = px
marker.pose.orientation.w = 1.0
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.a = 1.0
marker.color.r = rospy.get_param("~r", 1.0)
marker.color.g = rospy.get_param("~g", 1.0)
marker.color.b = rospy.get_param("~b", 1.0)
for face in faces:
print face
marker.points.append(self.pts[face[0]])
marker.points.append(self.pts[face[1]])
marker.points.append(self.pts[face[2]])
rospy.sleep(0.5)
self.pub.publish(marker)
rospy.sleep(0.5)
# make nodes and links and faces in SoftBody
total_mass = rospy.get_param("~mass", 2.0)
node_mass = total_mass / len(self.pts)
for pt in self.pts:
node = Node()
node.position.x = pt.x
node.position.y = pt.y
node.position.z = pt.z
node.mass = node_mass
body.node.append(node)
# center node
# This doesn't work well, would rather have a sphere
# built out of a layer of tetras
if False:
node = Node()
node.mass = node_mass
node.position.x = 0
node.position.y = 0
node.position.z = height
body.node.append(node)
center_ind = len(body.node) - 1
for i in range(0, len(body.node), 10):
link = Link()
link.node_indices[0] = i
link.node_indices[1] = center_ind
body.link.append(link)
link_map = {}
for i in range(len(faces)):
face = Face()
for j in range(3):
face.node_indices[j] = faces[i][j]
ind1 = faces[i][j]
ind2 = faces[i][(j + 1) % 3]
# prevent duplicate links
if ((ind1 in link_map.keys() and ind2 in link_map[ind1].keys()) or
(ind2 in link_map.keys() and ind1 in link_map[ind2].keys())):
continue
link = Link()
link.node_indices[0] = ind1
link.node_indices[1] = ind2
if not ind1 in link_map.keys():
link_map[ind1] = {}
link_map[ind1][ind2] = True
body.link.append(link)
body.face.append(face)
mat = Material()
mat.kLST = 0.9
mat.bending_distance = 2
body.material.append(mat)
body.config = make_soft_config()
body.config.kDF = 1.0
body.config.kDP = 0.004
body.config.kDG = 0.005
body.config.kPR = rospy.get_param("~pressure", 700.0) # pressure coefficient
body.config.kMT = 0.9
body.config.maxvolume = 0.5
body.margin = radius / 40.0
body.k_clusters = 8
body.randomize_constraints = True
rospy.wait_for_service('add_compound')
self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
add_compound_request = AddCompoundRequest()
add_compound_request.remove = False # rospy.get_param('~remove', False)
add_compound_request.soft_body.append(body)
rospy.loginfo(add_compound_request)
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)
def __init__(self):
length = rospy.get_param("~length", 0.5)
segments = rospy.get_param("~segments", 20)
py = rospy.get_param("~y", 0.0)
name = rospy.get_param("~name", "strand")
pz = 0.2
body = SoftBody()
body.name = name
body.pose.position.y = 0
body.pose.position.z = 0
body.pose.orientation.w = 1.0
# make nodes and links and faces in SoftBody
total_mass = rospy.get_param("~mass", 1.0)
node_mass = total_mass / float(segments)
node_length = length / float(segments)
x = 0
for i in range(segments):
node = Node()
node.position.x = x
node.position.y = py + 0.15
node.position.z = pz
node.mass = node_mass
body.node.append(node)
x += node_length
if i > 0:
link = Link()
link.node_indices[0] = i - 1
link.node_indices[1] = i
body.link.append(link)
# TODO(lucasw) need to anchor first and last elements to static (or possibly kinematic) rigid bodies
# TODO(lucasw) it would be nice to be able to tune these live
mat = Material()
mat.kLST = 0.9
mat.kAST = 0.1
# TODO(lucasw)
mat.bending_distance = 2
body.material.append(mat)
body.config = make_soft_config()
body.config.kDF = 1.0
body.config.kDP = 0.004
body.config.kDG = 0.005
# body.config.kPR = rospy.get_param("~pressure", 700.0) # pressure coefficient
body.config.kMT = 0.1
body.config.maxvolume = 0.5
body.margin = length / 40.0
body.k_clusters = 8
body.randomize_constraints = True
anchor0_body = make_rigid_box(name + "_anchor0",
0.0,
body.node[0].position.x - node_length,
py,
pz,
node_length,
node_length,
node_length * 10.0,
roll=0,
pitch=0,
yaw=0)
anchor0 = Anchor()
anchor0.node_index = 0
anchor0.rigid_body_name = anchor0_body.name
anchor0.influence = 0.05
anchor0.disable_collision_between_linked_bodies = True
anchor1_body = make_rigid_box(name + "_anchor1",
0.0,
body.node[-1].position.x + node_length,
py,
pz,
node_length,
node_length,
node_length * 10.0,
roll=0,
pitch=0,
yaw=0)
anchor1 = Anchor()
anchor1.node_index = len(body.node) - 1
anchor1.rigid_body_name = anchor1_body.name
anchor1.influence = anchor0.influence
anchor1.disable_collision_between_linked_bodies = True
body.anchor.append(anchor0)
body.anchor.append(anchor1)
add_compound_request = AddCompoundRequest()
add_compound_request.remove = False # rospy.get_param('~remove', False)
add_compound_request.soft_body.append(body)
add_compound_request.body.append(anchor0_body)
add_compound_request.body.append(anchor1_body)
rospy.wait_for_service('add_compound')
self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
rospy.loginfo(add_compound_request)
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)
def __init__(self):
self.pub = rospy.Publisher("marker", Marker, queue_size=2)
radius = rospy.get_param("~radius", 0.5)
levels = rospy.get_param("~levels", 3)
px = rospy.get_param("~x", 0.0)
name = rospy.get_param("~name", "ball")
body = SoftBody()
body.name = name
body.pose.position.x = px
body.pose.position.z = 0.2
body.pose.orientation.w = 1.0
# from http://blog.andreaskahler.com/2009/06/creating-icosphere-mesh-in-code.html
icost = (1.0 + math.sqrt(5.0)) / 2.0
tmp_radius = math.sqrt(1.0 + icost * icost)
scale = radius / tmp_radius
self.pts = []
self.subpts = {}
pt = Point()
self.pts.append(Point(-1, icost, 0))
self.pts.append(Point(1, icost, 0))
self.pts.append(Point(-1, -icost, 0))
self.pts.append(Point(1, -icost, 0))
self.pts.append(Point(0, -1, icost))
self.pts.append(Point(0, 1, icost))
self.pts.append(Point(0, -1, -icost))
self.pts.append(Point(0, 1, -icost))
self.pts.append(Point(icost, 0, -1))
self.pts.append(Point(icost, 0, 1))
self.pts.append(Point(-icost, 0, -1))
self.pts.append(Point(-icost, 0, 1))
# first construct an icosohedron
# rospy.loginfo(ico)
faces = []
faces.append([0, 11, 5])
faces.append([0, 5, 1])
faces.append([0, 1, 7])
faces.append([0, 7, 10])
faces.append([0, 10, 11])
if True:
faces.append([1, 5, 9])
faces.append([5, 11, 4])
faces.append([11, 10, 2])
faces.append([10, 7, 6])
faces.append([7, 1, 8])
faces.append([3, 9, 4])
faces.append([3, 4, 2])
faces.append([3, 2, 6])
faces.append([3, 6, 8])
faces.append([3, 8, 9])
faces.append([4, 9, 5])
faces.append([2, 4, 11])
faces.append([6, 2, 10])
faces.append([8, 6, 7])
faces.append([9, 8, 1])
for i in range(levels):
faces = self.subdivide(faces)
height = rospy.get_param("~height", radius * 1.1)
print len(self.pts)
# scale all points to be on sphere of desired radius
for i in range(len(self.pts)):
pt = self.pts[i]
scale = radius / math.sqrt(pt.x * pt.x + pt.y * pt.y + pt.z * pt.z)
pt.x *= scale
pt.y *= scale
pt.z *= scale
pt.z += height
self.pts[i] = pt
if False:
marker = Marker()
marker.header.frame_id = rospy.get_param("~frame_id", "map")
marker.ns = name
marker.id = 1
marker.type = Marker.TRIANGLE_LIST
marker.action = Marker.ADD
marker.pose.position.x = px
marker.pose.orientation.w = 1.0
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.a = 1.0
marker.color.r = rospy.get_param("~r", 1.0)
marker.color.g = rospy.get_param("~g", 1.0)
marker.color.b = rospy.get_param("~b", 1.0)
for face in faces:
print face
marker.points.append(self.pts[face[0]])
marker.points.append(self.pts[face[1]])
marker.points.append(self.pts[face[2]])
rospy.sleep(0.5)
self.pub.publish(marker)
rospy.sleep(0.5)
# make nodes and links and faces in SoftBody
total_mass = rospy.get_param("~mass", 2.0)
node_mass = total_mass / len(self.pts)
for pt in self.pts:
node = Node()
node.position.x = pt.x
node.position.y = pt.y
node.position.z = pt.z
node.mass = node_mass
body.node.append(node)
# center node
# This doesn't work well, would rather have a sphere
# built out of a layer of tetras
if False:
node = Node()
node.mass = node_mass
node.position.x = 0
node.position.y = 0
node.position.z = height
body.node.append(node)
center_ind = len(body.node) - 1
for i in range(0, len(body.node), 10):
link = Link()
link.node_indices[0] = i
link.node_indices[1] = center_ind
body.link.append(link)
link_map = {}
for i in range(len(faces)):
face = Face()
for j in range(3):
face.node_indices[j] = faces[i][j]
ind1 = faces[i][j]
ind2 = faces[i][(j + 1) % 3]
# prevent duplicate links
ind_in_map = ((ind1 in link_map.keys() and ind2 in link_map[ind1].keys()) or
(ind2 in link_map.keys() and ind1 in link_map[ind2].keys()))
if ind_in_map:
continue
link = Link()
link.node_indices[0] = ind1
link.node_indices[1] = ind2
if ind1 not in link_map.keys():
link_map[ind1] = {}
link_map[ind1][ind2] = True
body.link.append(link)
body.face.append(face)
mat = Material()
mat.kLST = 0.9
mat.bending_distance = 2
body.material.append(mat)
body.config = make_soft_config()
body.config.kDF = 1.0
body.config.kDP = 0.004
body.config.kDG = 0.005
body.config.kPR = rospy.get_param("~pressure", 700.0) # pressure coefficient
body.config.kMT = 0.9
body.config.maxvolume = 0.5
body.margin = radius / 40.0
body.k_clusters = 8
body.randomize_constraints = True
rospy.wait_for_service('add_compound')
self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
add_compound_request = AddCompoundRequest()
add_compound_request.remove = False # rospy.get_param('~remove', False)
add_compound_request.soft_body.append(body)
rospy.loginfo(add_compound_request)
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)
def __init__(self):
length = rospy.get_param("~length", 0.5)
segments = rospy.get_param("~segments", 20)
py = rospy.get_param("~y", 0.0)
name = rospy.get_param("~name", "strand")
pz = 0.2
body = SoftBody()
body.name = name
body.pose.position.y = 0
body.pose.position.z = 0
body.pose.orientation.w = 1.0
# make nodes and links and faces in SoftBody
total_mass = rospy.get_param("~mass", 1.0)
node_mass = total_mass / float(segments)
node_length = length / float(segments)
x = 0
for i in range(segments):
node = Node()
node.position.x = x
node.position.y = py + 0.15
node.position.z = pz
node.mass = node_mass
body.node.append(node)
x += node_length
if i > 0:
link = Link()
link.node_indices[0] = i - 1
link.node_indices[1] = i
body.link.append(link)
# TODO(lucasw) need to anchor first and last elements to static (or possibly kinematic) rigid bodies
# TODO(lucasw) it would be nice to be able to tune these live
mat = Material()
mat.kLST = 0.9
mat.kAST = 0.1
# TODO(lucasw)
mat.bending_distance = 2
body.material.append(mat)
body.config = make_soft_config()
body.config.kDF = 1.0
body.config.kDP = 0.004
body.config.kDG = 0.005
# body.config.kPR = rospy.get_param("~pressure", 700.0) # pressure coefficient
body.config.kMT = 0.1
body.config.maxvolume = 0.5
body.margin = length / 40.0
body.k_clusters = 8
body.randomize_constraints = True
anchor0_body = make_rigid_box(name + "_anchor0", 0.0,
body.node[0].position.x - node_length, py, pz,
node_length, node_length, node_length * 10.0,
roll=0, pitch=0, yaw=0)
anchor0 = Anchor()
anchor0.node_index = 0
anchor0.rigid_body_name = anchor0_body.name
anchor0.influence = 0.05
anchor0.disable_collision_between_linked_bodies = True
anchor1_body = make_rigid_box(name + "_anchor1", 0.0,
body.node[-1].position.x + node_length, py, pz,
node_length, node_length, node_length * 10.0,
roll=0, pitch=0, yaw=0)
anchor1 = Anchor()
anchor1.node_index = len(body.node) - 1
anchor1.rigid_body_name = anchor1_body.name
anchor1.influence = anchor0.influence
anchor1.disable_collision_between_linked_bodies = True
body.anchor.append(anchor0)
body.anchor.append(anchor1)
add_compound_request = AddCompoundRequest()
add_compound_request.remove = False # rospy.get_param('~remove', False)
add_compound_request.soft_body.append(body)
add_compound_request.body.append(anchor0_body)
add_compound_request.body.append(anchor1_body)
rospy.wait_for_service('add_compound')
self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
rospy.loginfo(add_compound_request)
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)
def __init__(self):
rospy.wait_for_service('add_compound')
self.add_compound = rospy.ServiceProxy('add_compound', AddCompound)
add_compound_request = AddCompoundRequest()
add_compound_request.remove = rospy.get_param('~remove', False)
xs = rospy.get_param("~x", 0.0)
ys = rospy.get_param("~y", 0.0)
zs = rospy.get_param("~z", 1.0)
body = SoftBody()
body.name = "pyramid"
body.config = make_soft_config()
mass = 0.5
n1 = Node()
n1.mass = mass
n1.position.x = xs
n1.position.y = ys
n1.position.z = zs
body.node.append(n1)
n1 = Node()
n1.mass = mass
n1.position.x = xs + 1.0
n1.position.y = ys
n1.position.z = zs
body.node.append(n1)
n1 = Node()
n1.mass = mass
n1.position.x = xs
n1.position.y = ys + 1.0
n1.position.z = zs
body.node.append(n1)
n1 = Node()
n1.mass = mass
n1.position.x = xs
n1.position.y = ys
n1.position.z = zs + 1.0
body.node.append(n1)
l1 = Link()
l1.node_indices[0] = 0
l1.node_indices[1] = 1
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 0
l1.node_indices[1] = 2
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 0
l1.node_indices[1] = 3
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 1
l1.node_indices[1] = 2
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 1
l1.node_indices[1] = 3
body.link.append(l1)
l1 = Link()
l1.node_indices[0] = 2
l1.node_indices[1] = 3
body.link.append(l1)
mat = Material()
mat.kLST = 0.15
mat.kVST = 0.1
mat.kAST = 0.1
body.material.append(mat)
body.k_clusters = 8
body.margin = 0.05
add_compound_request.soft_body.append(body)
if False:
# make the top cylinder plate
top_plate = Body()
top_plate.name = "top_plate"
top_plate.mass = 0.3
rot90 = tf.transformations.quaternion_from_euler(math.pi/2.0, 0, 0)
radius = 1.5
thickness = 0.5
top_plate.pose.orientation.x = rot90[0]
top_plate.pose.orientation.y = rot90[1]
top_plate.pose.orientation.z = rot90[2]
top_plate.pose.orientation.w = rot90[3]
top_plate.pose.position.x = 0
top_plate.pose.position.y = 0
top_plate.pose.position.z = 1.7
top_plate.type = Body.CYLINDER
top_plate.scale.x = radius
top_plate.scale.y = thickness
top_plate.scale.z = radius
add_compound_request.body.append(top_plate)
rospy.loginfo(add_compound_request)
try:
add_compound_response = self.add_compound(add_compound_request)
rospy.loginfo(add_compound_response)
except rospy.service.ServiceException as e:
rospy.logerr(e)