4 bar linkage

examples/lecture06_decomposition.py

@@ -0,0 +1,47 @@
+import os, time
+import numpy as np
+import mengine as m
+
+# Create environment and ground plane
+env = m.Env()
+ground = m.Ground()
+env.set_gui_camera(look_at_pos=[0, 0, 0])
+
+# Show global coordinate frame
+m.visualize_coordinate_frame()
+
+cube = m.Shape(m.Box(half_extents=[0.1]*3), static=True, position=[0.2, 0, 0], orientation=[0, 0, 0, 1], rgba=[0, 1, 0, 0.5])
+p, q = cube.get_base_pos_orient()
+
+# Rotate around the origin first
+for i in range(100):
+    p2, q2 = m.multiply_transforms([0, 0, 0], [0, 0, np.radians(i)], p, q)
+    cube.set_base_pos_orient(p2, q2)
+    m.step_simulation(realtime=True)
+
+# Then translate along x-axis
+for i in range(100):
+    # p3, q3 = m.multiply_transforms(p2, q2, [i*0.005, 0, 0], [0, 0, 0])
+    p3, q3 = m.multiply_transforms([i*0.005, 0, 0], [0, 0, 0], p2, q2)
+    cube.set_base_pos_orient(p3, q3)
+    m.step_simulation(realtime=True)
+
+
+cube2 = m.Shape(m.Box(half_extents=[0.1]*3), static=True, position=[0.2, 0, 0], orientation=[0, 0, 0, 1], rgba=[0, 0, 1, 0.5])
+p, q = cube2.get_base_pos_orient()
+
+# Translate along x-axis first
+for i in range(100):
+    # p2, q2 = m.multiply_transforms(p, q, [i*0.005, 0, 0], [0, 0, 0])
+    p2, q2 = m.multiply_transforms([i*0.005, 0, 0], [0, 0, 0], p, q)
+    cube2.set_base_pos_orient(p2, q2)
+    m.step_simulation(realtime=True)
+
+# Then rotate around the origin
+for i in range(100):
+    p3, q3 = m.multiply_transforms([0, 0, 0], [0, 0, np.radians(i)], p2, q2)
+    cube2.set_base_pos_orient(p3, q3)
+    m.step_simulation(realtime=True)
+
+while True:
+    m.step_simulation(realtime=True)

examples/lecture06_fourbarlinkage.py

@@ -0,0 +1,21 @@
+import os, time
+import numpy as np
+import mengine as m
+
+# Create environment and ground plane
+env = m.Env()
+ground = m.Ground()
+env.set_gui_camera(look_at_pos=[0, 0, 0], yaw=30)
+
+fbl = m.URDF(filename=os.path.join(m.directory, 'fourbarlinkage.urdf'), static=True, position=[0, 0, 0.3], orientation=[0, 0, 0, 1])
+fbl.controllable_joints = [0, 1, 2]
+
+fbl.create_constraint(parent_link=1, child=fbl, child_link=4, joint_type=m.p.JOINT_POINT2POINT, joint_axis=[0, 0, 0], parent_pos=[0, 0, 0], child_pos=[0, 0, 0])
+
+# m.visualize_coordinate_frame()
+
+for i in range(10000):
+    j = np.radians(i)
+    fbl.control([j]*3)
+    m.step_simulation(realtime=True)
+

examples/lecture06_ic.py

@@ -0,0 +1,69 @@
+import os, time
+import numpy as np
+import mengine as m
+
+# Create environment and ground plane
+env = m.Env()
+ground = m.Ground()
+env.set_gui_camera(look_at_pos=[0, 0, 0])
+
+cube = m.Shape(m.Box(half_extents=[0.1]*3), static=True, position=[0.2, 0, 0], orientation=[0, 0, 0, 1], rgba=[0, 1, 0, 0.5])
+p, q = cube.get_base_pos_orient()
+
+local_points = [[0.1, 0.1, 0.1], [0.1, -0.1, 0.1], [0.1, 0, 0.1]]
+global_points = []
+previous_global_points = []
+lines = []
+
+for position in local_points:
+    global_points.append(cube.local_to_global_coordinate_frame(position)[0])
+    previous_global_points.append(global_points[-1])
+    point = m.Shape(m.Sphere(radius=0.02), static=True, position=global_points[-1], rgba=[0, 0, 1, 1])
+
+rotation_center = [0.2, -0.2, 0]
+# rotation_center = [0, 0, 0]
+point_rc = m.Shape(m.Sphere(radius=0.02), static=True, position=rotation_center, rgba=[1, 0, 0, 1])
+
+m.step_simulation(steps=30, realtime=True)
+
+# Plot instantaneous rotation center
+for i in range(100):
+    p2, q2 = m.multiply_transforms(rotation_center, [0, 0, np.radians(i+1)], p-rotation_center, q)
+    cube.set_base_pos_orient(p2, q2)
+
+    for i, (position, global_position, previous_global_position) in enumerate(zip(local_points, global_points, previous_global_points)):
+        p_new = cube.local_to_global_coordinate_frame(position)[0]
+        ic_vector_of_motion = p_new - previous_global_position
+        ic_bisector = np.cross(ic_vector_of_motion, [0,0,1])
+        ic_bisector = ic_bisector / np.linalg.norm(ic_bisector)
+        previous_global_points[i] = p_new
+
+        if i >= len(lines):
+            lines.append([])
+            # lines[i].append(m.Line(global_position, p_new, radius=0.005, rgba=[1, 1, 1, 0.5]))
+            # lines[i].append(m.Line(midpoint, midpoint-bisector/2, radius=0.005, rgba=[1, 0, 0, 0.5]))
+            lines[i].append(m.Line(p_new, p_new+ic_vector_of_motion*10, radius=0.005, rgba=[1, 0, 0, 0.5]))
+            lines[i].append(m.Line(p_new, p_new-ic_bisector/2, radius=0.005, rgba=[0, 0, 1, 0.5]))
+        else:
+            # m.clear_visual_item(lines[i][0])
+            # lines[i][0] = m.Line(global_position, p_new, radius=0.005, rgba=[1, 1, 1, 0.5])
+            # lines[i][1] = m.Line(midpoint, midpoint-bisector/2, radius=0.005, rgba=[1, 0, 0, 0.5], replace_line=lines[i][1])
+            lines[i][0] = m.Line(p_new, p_new+ic_vector_of_motion*10, radius=0.005, rgba=[1, 0, 0, 0.5], replace_line=lines[i][0])
+            lines[i][1] = m.Line(p_new, p_new-ic_bisector/2, radius=0.005, rgba=[0, 0, 1, 0.5], replace_line=lines[i][1])
+
+    m.step_simulation(realtime=True)
+
+# Plot the intersecting lines for the rotation center
+for i, (position, global_position, previous_global_position) in enumerate(zip(local_points, global_points, previous_global_points)):
+    p_new = cube.local_to_global_coordinate_frame(position)[0]
+    vector_of_motion = p_new - global_position
+    midpoint = global_position + vector_of_motion/2
+    bisector = np.cross(vector_of_motion, [0,0,1])
+    bisector = bisector / np.linalg.norm(bisector)
+
+    m.Line(global_position, p_new, radius=0.005, rgba=[1, 1, 1, 0.5])
+    m.Line(midpoint, midpoint-bisector/2, radius=0.005, rgba=[1, 0, 0, 0.5])
+
+while True:
+    m.step_simulation(realtime=True)
+

mengine/assets/fourbarlinkage.urdf

@@ -0,0 +1,100 @@
+<?xml version="1.0"?>
+<robot name="simple_arm">
+
+  <!-- Define materials -->
+  <material name="white">
+    <color rgba="1 1 1 1"/>
+  </material>
+  <material name="red">
+    <color rgba="1 0 0 1"/>
+  </material>
+
+
+  <!-- Define links -->
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 1.5707 0"/>
+      <geometry>
+        <capsule length="0.2" radius="0.02"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+
+  <link name="link2">
+    <visual>
+     <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <capsule length="0.2" radius="0.02"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+
+  <link name="link3">
+    <visual>
+     <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <capsule length="0.2" radius="0.02"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+
+  <link name="link4">
+    <visual>
+     <origin xyz="0 -0.2 0" rpy="1.5707 0 0"/>
+      <geometry>
+        <capsule length="0.4" radius="0.02"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+
+  <link name="link5"/>
+
+  <link name="link6"/>
+
+
+  <!-- Define joints -->
+  <joint name="joint1" type="revolute">
+    <parent link="base_link"/>
+    <child link="link2"/>
+    <origin xyz="0.1 0 0" rpy="0 0 1.5707"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100" velocity="10"/>
+  </joint>
+
+  <joint name="joint2" type="revolute">
+    <parent link="base_link"/>
+    <child link="link3"/>
+    <origin xyz="-0.1 0 0" rpy="0 0 1.5707"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100" velocity="10"/>
+  </joint>
+
+  <joint name="joint3" type="revolute">
+    <parent link="link3"/>
+    <child link="link4"/>
+    <origin xyz="0 0 0.2" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100" velocity="10"/>
+  </joint>
+
+  <joint name="joint4" type="revolute">
+    <parent link="link4"/>
+    <child link="link5"/>
+    <origin xyz="0 -0.2 0" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100" velocity="10"/>
+  </joint>
+
+  <joint name="joint5" type="revolute">
+    <parent link="link2"/>
+    <child link="link6"/>
+    <origin xyz="0 0 0.2" rpy="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="100" velocity="10"/>
+  </joint>
+
+</robot>

mengine/env.py

@@ -294,15 +294,36 @@ def create_body(shape=p.GEOM_CAPSULE, radius=0, length=0, position_offset=[0, 0,
         # collision_shape = p.createCollisionShape(shape, radius=radius, height=length, collisionFramePosition=position_offset, collisionFrameOrientation=orientation, physicsClientId=env.id)
         # return collision_shape, visual_shape
 
-    link_c1, link_v1 = create_body(radius=0.01, length=0.05)
-    link_c2, link_v2 = create_body(radius=0.01, length=0.05)
-    link_c3, link_v3 = create_body(radius=0.01, length=0.05)
-    link_c4, link_v4 = create_body(radius=0.01, length=0.05)
-
-    link_p1, link_p2, link_p3, link_p4 = [[0, 0, 0.05]]*4
-    link_o1, link_o2, link_o3, link_o4 = [[0, 0, 0, 1]]*4
-
-    linkMasses = [1, 1, 1]
+    # link_c1, link_v1 = create_body(radius=0.01, length=0.05)
+    # link_c2, link_v2 = create_body(radius=0.01, length=0.05)
+    # link_c3, link_v3 = create_body(radius=0.01, length=0.05)
+    # link_c4, link_v4 = create_body(radius=0.01, length=0.05)
+
+    link_c1, link_c2, link_c3, link_c4 = [-1]*4
+
+    link_v1 = p.createVisualShape(p.GEOM_CAPSULE, radius=0.02, length=0.2, rgbaColor=[1, 1, 1, 1], visualFramePosition=[0, 0, 0], visualFrameOrientation=[0, 0, 0, 1], physicsClientId=env.id)
+    link_v2 = p.createVisualShape(p.GEOM_CAPSULE, radius=0.02, length=0.2, rgbaColor=[1, 1, 1, 1], visualFramePosition=[0, 0, 0.1], visualFrameOrientation=[0, 0, 0, 1], physicsClientId=env.id)
+    link_v3 = p.createVisualShape(p.GEOM_CAPSULE, radius=0.02, length=0.4, rgbaColor=[1, 1, 1, 1], visualFramePosition=[0, 0, 0.2], visualFrameOrientation=[0, 0, 0, 1], physicsClientId=env.id)
+    link_v4 = p.createVisualShape(p.GEOM_CAPSULE, radius=0.02, length=0.2, rgbaColor=[1, 1, 1, 1], visualFramePosition=[0, 0, 0.1], visualFrameOrientation=[0, 0, 0, 1], physicsClientId=env.id)
+
+    # # link_v1 = p.createVisualShape(p.GEOM_CAPSULE, radius=0.02, length=0.2, rgbaColor=[1, 1, 1, 1], visualFramePosition=[0, 0, 0], visualFrameOrientation=get_quaternion([0, np.pi/2, 0]), physicsClientId=env.id)
+    # link_v1 = p.createVisualShape(p.GEOM_CAPSULE, radius=0.02, length=0.2, rgbaColor=[1, 1, 1, 1], visualFramePosition=[0, 0, 0], visualFrameOrientation=[0, 0, 0, 1], physicsClientId=env.id)
+    # link_v2 = p.createVisualShape(p.GEOM_CAPSULE, radius=0.02, length=0.2, rgbaColor=[1, 1, 1, 1], visualFramePosition=[0, 0, 0.1], visualFrameOrientation=[0, 0, 0, 1], physicsClientId=env.id)
+    # link_v3 = p.createVisualShape(p.GEOM_CAPSULE, radius=0.02, length=0.2, rgbaColor=[1, 1, 1, 1], visualFramePosition=[0, 0, 0.1], visualFrameOrientation=[0, 0, 0, 1], physicsClientId=env.id)
+    # # link_v4 = p.createVisualShape(p.GEOM_CAPSULE, radius=0.02, length=0.2, rgbaColor=[1, 1, 1, 1], visualFramePosition=[0, -0.2, 0], visualFrameOrientation=get_quaternion([np.pi/2, 0, 0]), physicsClientId=env.id)
+    # link_v4 = p.createVisualShape(p.GEOM_CAPSULE, radius=0.02, length=0.2, rgbaColor=[1, 1, 1, 1], visualFramePosition=[0, 0, 0.1], visualFrameOrientation=[0, 0, 0, 1], physicsClientId=env.id)
+
+    # link_p1, link_o1 = [0, 0, 0], get_quaternion([0, np.pi/2, 0])
+    # link_p2, link_o2 = [0, 0, 0.1], get_quaternion([0, 0, np.pi/2])
+    # link_p3, link_o3 = [0, 0, -0.1], get_quaternion([0, 0, np.pi/2])
+    # link_p4, link_o4 = [0, 0, 0.2], get_quaternion([np.pi/2, 0, 0])
+
+    link_p1, link_o1 = [0, 0, 0], get_quaternion([0, -np.pi/2, 0])
+    link_p2, link_o2 = [0, 0, 0.1], get_quaternion([0, 0, np.pi/2])
+    link_p3, link_o3 = [0, 0, 0.2], [0, 0, 0, 1]
+    link_p4, link_o4 = [0, 0, 0.2], [0, 0, 0, 1]
+
+    linkMasses = [1]*3
     linkCollisionShapeIndices = [link_c2, link_c3, link_c4]
     linkVisualShapeIndices = [link_v2, link_v3, link_v4]
     linkPositions = [link_p2, link_p3, link_p4]
@@ -315,3 +336,4 @@ def create_body(shape=p.GEOM_CAPSULE, radius=0, length=0, position_offset=[0, 0,
 
     body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=link_c1, baseVisualShapeIndex=link_v1, basePosition=link_p1, baseOrientation=link_o1, linkMasses=linkMasses, linkCollisionShapeIndices=linkCollisionShapeIndices, linkVisualShapeIndices=linkVisualShapeIndices, linkPositions=linkPositions, linkOrientations=linkOrientations, linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations, linkParentIndices=linkParentIndices, linkJointTypes=linkJointTypes, linkJointAxis=linkJointAxis, physicsClientId=env.id)
     return Body(body, env, controllable_joints=[0, 1, 2])
+

setup.py

@@ -18,7 +18,7 @@
     packages=find_packages(),
     python_requires='>=3',
     # install_requires=['gym>=0.2.3', 'pybullet @ git+https://github.com/Zackory/bullet3.git@pybullet_3_0_9#egg=pybullet', 'numpy', 'keras==2.3.0', 'tensorflow==1.14.0', 'h5py==2.10.0', 'smplx', 'trimesh', 'ray[rllib]', 'numpngw', 'tensorflow-probability==0.7.0', 'matplotlib'] + ['screeninfo==0.6.1' if sys.version_info >= (3, 6) else 'screeninfo==0.2'],
-    install_requires=['pybullet', 'numpy', 'numpngw'] + ['screeninfo==0.6.1' if sys.version_info >= (3, 6) else 'screeninfo==0.2'],
+    install_requires=['pybullet', 'numpy', 'scipy', 'numpngw'] + ['screeninfo==0.6.1' if sys.version_info >= (3, 6) else 'screeninfo==0.2'],
     # description='Physics simulation for assistive robotics and human-robot interaction.',
     # long_description=long_description,
     long_description_content_type="text/markdown",