cm

python/examples/clik.py

@@ -35,7 +35,7 @@ if __name__ == "__main__":
         moveL(args, robot, Mgoal)
     else:
         goal_transform = pin.SE3.Identity()
-        goal_transform.translation[1] = 0.1
+        #goal_transform.translation[1] = 0.1
         moveLDualArm(args, robot, Mgoal, goal_transform)
     robot.closeGripper()
     robot.openGripper()

python/ur_simple_control/managers.py

@@ -281,7 +281,7 @@ class ControlLoopManager:
         if i % 20 == 0:
             # don't send what wasn't ready
             if self.args.visualize_manipulator:
-                if self.robot_manage.robot_name != "yumi":
+                if self.robot_manager.robot_name != "yumi":
                     self.robot_manager.visualizer_manager.sendCommand({"q" : self.robot_manager.q,
                                                           "T_w_e" : self.robot_manager.getT_w_e()})
                 else:

python/ur_simple_control/util/get_model.py

@@ -181,13 +181,61 @@ def get_yumi_model():
     # this command just calls the ones below it. both are kept here
     # in case pinocchio people decide to change their api.
     #model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
-    model = pin.buildModelFromUrdf(urdf_path_absolute)
-    visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute)
-    collision_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION, None, mesh_dir_absolute)
+    model_arms = pin.buildModelFromUrdf(urdf_path_absolute)
+    visual_model_arms = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute)
+    collision_model_arms = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION, None, mesh_dir_absolute)
 
-    data = pin.Data(model)
+    data_arms = pin.Data(model)
+
+    # mobile base as planar joint (there's probably a better
+    # option but whatever right now)
+    model_mobile_base = pin.Model()
+    model_mobile_base.name = "mobile_base"
+    geom_model_mobile_base = pin.GeometryModel()
+    joint_name = "mobile_base_planar_joint"
+    parent_id = 0
+    # TEST
+    joint_placement = pin.SE3.Identity()
+    #joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0) 
+    #joint_placement.translation[2] = 0.2
+    # TODO TODO TODO TODO TODO TODO TODO TODO
+    # TODO: heron is actually a differential drive,
+    # meaning that it is not a planar joint.
+    # we could put in a prismatic + revolute joint
+    # as the base (both joints being at same position),
+    # and that should work for our purposes.
+    # this makes sense for initial testing
+    # because mobile yumi's base is a planar joint
+    MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(parent_id, pin.JointModelPlanar(), 
+            joint_placement.copy(), joint_name)
+    # we should immediately set velocity limits.
+    # there are no position limit by default and that is what we want.
+    model_mobile_base.velocityLimit[0] = 2
+    model_mobile_base.velocityLimit[1] = 2
+    model_mobile_base.velocityLimit[2] = 2
+    model_mobile_base.effortLimit[0] = 200
+    model_mobile_base.effortLimit[1] = 2
+    model_mobile_base.effortLimit[2] = 200
+
+    # pretty much random numbers
+    # TODO: find heron (mir) numbers
+    body_inertia = pin.Inertia.FromBox(30, 0.5, 0.3, 0.4)
+    # maybe change placement to sth else depending on where its grasped
+    model_mobile_base.appendBodyToJoint(MOBILE_BASE_JOINT_ID, body_inertia, pin.SE3.Identity()) 
+    box_shape = fcl.Box(0.5, 0.3, 0.4) 
+    body_placement = pin.SE3.Identity()
+    geometry_mobile_base = pin.GeometryObject("box_shape", MOBILE_BASE_JOINT_ID, box_shape, body_placement.copy())
+
+    geometry_mobile_base.meshColor = np.array([1., 0.1, 0.1, 1.])
+    geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
+
+    # have to add the frame manually
+    model_mobile_base.addFrame(pin.Frame('mobile_base',MOBILE_BASE_JOINT_ID,0,joint_placement.copy(),pin.FrameType.JOINT) )
+
+    # frame-index should be 1
+    model, visual_model = pin.appendModel(model_mobile_base, model_arms, geom_model_mobile_base, visual_model_arms, 1, pin.SE3.Identity())
+    return model, visual_model.copy(), visual_model, data
 
-    return model, collision_model, visual_model, data
 
 def heron_approximation():
     # arm + gripper