def main(argv):
if len(argv) > 1:
raise app.UsageError("Too many command-line arguments.")
w = pydiffphys.TinyWorld()
w.gravity = pydiffphys.TinyVector3(0, 0, -9.81)
g = w.gravity
print("g=", g.z)
plane = pydiffphys.TinyPlane()
sphere = pydiffphys.TinySphere(5.0)
d = w.get_collision_dispatcher()
mass = 0.0
b1 = pydiffphys.TinyRigidBody(mass, plane)
mass = 1.0
b2 = pydiffphys.TinyRigidBody(mass, sphere)
print("b.linear_velocity=", b1.linear_velocity)
pos_a = pydiffphys.TinyVector3(0, 0, 0)
orn_a = pydiffphys.TinyQuaternion(0, 0, 0, 1)
pos_b = pydiffphys.TinyVector3(0, 0, 50)
orn_b = pydiffphys.TinyQuaternion(0, 0, 0, 1)
pose_a = pydiffphys.TinyPose(pos_a, orn_a)
pose_b = pydiffphys.TinyPose(pos_b, orn_b)
b1.world_pose = pose_a
b2.world_pose = pose_b
steps = 1000
for step in range(steps):
b2.apply_gravity(g)
dt = 1. / 240.
b2.apply_force_impulse(dt)
b2.clear_forces()
bodies = [b1, b2]
rb_contacts = w.compute_contacts_rigid_body(bodies, d)
num_iter = 50
solver = pydiffphys.TinyConstraintSolver()
for _ in range(num_iter):
for contact in rb_contacts:
solver.resolve_collision(contact, dt)
for body in bodies:
body.integrate(dt)
print("step=", step, ": b2.world_pose.position=", b2.world_pose.position,
", b2.linear_velocity=", b2.linear_velocity)
def create_multi_body(mass, collision_shapes, is_floating, world): urdf = pd.TinyUrdfStructures() base_link = pd.TinyUrdfLink() base_link.link_name = "plane_link" inertial = pd.TinyUrdfInertial() inertial.mass = mass inertial.inertia_xxyyzz = pd.TinyVector3(mass,mass,mass) base_link.urdf_inertial = inertial base_link.urdf_collision_shapes = collision_shapes urdf.base_links = [base_link] mb = pd.TinyMultiBody(is_floating) urdf2mb = pd.UrdfToMultiBody2() res = urdf2mb.convert2(urdf, world, mb) return mb
def create_multi_body(link_name, mass, collision_shapes, visual_shapes,
is_floating, mc, world):
urdf = pd.TinyUrdfStructures()
base_link = pd.TinyUrdfLink()
base_link.link_name = link_name
inertial = pd.TinyUrdfInertial()
inertial.mass = mass
inertial.inertia_xxyyzz = pd.TinyVector3(mass, mass, mass)
base_link.urdf_inertial = inertial
base_link.urdf_collision_shapes = collision_shapes
base_link.urdf_visual_shapes = visual_shapes
urdf.base_links = [base_link]
mb = pd.TinyMultiBody(is_floating)
urdf2mb = pd.UrdfToMultiBody2()
res = urdf2mb.convert2(urdf, world, mb)
b2vis = meshcat_utils_dp.convert_visuals(urdf, None, vis)
return mb, b2vis
def main(argv):
if len(argv) > 1:
raise app.UsageError("Too many command-line arguments.")
w = pydiffphys.TinyWorld()
w.gravity = pydiffphys.TinyVector3(0, 0, -9.81)
g = w.gravity
print("g=", g.z)
s = pydiffphys.TinySphere(2.0)
p = pydiffphys.TinyPlane()
mass = 1.0
b = pydiffphys.TinyRigidBody(mass, s)
b.world_pose.position.z = 10
#print(s.get_radius())
print("s.get_type()=", s.get_type())
print("p.get_type()=", p.get_type())
for i in range(10):
w.step(0.1)
print("b.m_world_pose", b.world_pose.position.z)
def convert_vec(self, vec):
vec = dp.TinyVector3(vec[0], vec[1], vec[2])
return vec
inertial.mass = mass
inertial.inertia_xxyyzz = pd.TinyVector3(mass,mass,mass)
base_link.urdf_inertial = inertial
base_link.urdf_collision_shapes = collision_shapes
urdf.base_links = [base_link]
mb = pd.TinyMultiBody(is_floating)
urdf2mb = pd.UrdfToMultiBody2()
res = urdf2mb.convert2(urdf, world, mb)
return mb
world = pd.TinyWorld()
collision_shape = pd.TinyUrdfCollision()
collision_shape.geometry.geom_type = pd.PLANE_TYPE
collision_shape.origin_xyz = pd.TinyVector3(0.,0.,2.)
plane_mb = create_multi_body(0, [collision_shape], False, world)
collision_shape = pd.TinyUrdfCollision()
collision_shape.geometry.geom_type = pd.SPHERE_TYPE
collision_shape.geometry.sphere.radius = 2.0
sphere_mb = create_multi_body(2.0, [collision_shape], True, world)
sphere_mb.set_base_position(pd.TinyVector3(0.,0.,25.))
dt = 1./240.
mb_solver = pd.TinyMultiBodyConstraintSolver()
while 1:
sphere_mb.forward_kinematics()
print("sphere_mb.q=",sphere_mb.q)
#now convert urdf structs to diffphys urdf structs
med0.convert_urdf_structures()
print("conversion to TinyUrdfStructures done!")
texture_path = os.path.join(pd.getDataPath(), "laikago/laikago_tex.jpg")
laikago_vis = meshcat_utils_dp.convert_visuals(med0.urdf_structs, texture_path,
vis)
print("convert_visuals done")
urdf2mb = dp.UrdfToMultiBody2()
is_floating = True
laikago_mb = dp.TinyMultiBody(is_floating)
res = urdf2mb.convert2(med0.urdf_structs, world, laikago_mb)
laikago_mb.set_base_position(dp.TinyVector3(0., 0., 2.))
mb_solver = dp.TinyMultiBodyConstraintSolver()
dt = 1. / 1000.
#p0.setGravity(0,0,-10)
while p0.isConnected():
laikago_mb.forward_kinematics()
laikago_mb.forward_dynamics(dp.TinyVector3(0., 0., -10.))
laikago_mb.integrate_q(dt)
multi_bodies = [plane_mb, laikago_mb]
contacts = world.compute_contacts_multi_body(multi_bodies, dispatcher)
#collision solver
