diff --git a/python/examples/.clik.py.swp b/python/examples/.clik.py.swp index 1db23ff573d61056928c496f846f9fca07b40603..cfa969c37f3da44874ff263c98f9fbf4d98d91ac 100644 Binary files a/python/examples/.clik.py.swp and b/python/examples/.clik.py.swp differ diff --git a/python/examples/clik.py b/python/examples/clik.py index d450674c4084408485f9616552ddb4f4bcf1f921..1c3a4d7faf5ce88f54eaf4e0ae1a401b2f95688c 100644 --- a/python/examples/clik.py +++ b/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() diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp index 1207a0999fb68434c4d8ed30f2d2bc6621e58b20..e12a3e0820c44510204d6fa502fa0c6198de9deb 100644 Binary files a/python/ur_simple_control/.managers.py.swp and b/python/ur_simple_control/.managers.py.swp differ diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py index 82b0c4c7df00102398eda0a710928cf51f295afc..3b5c8bf58f00147ff3ab8a80a1c1262b7e5e1655 100644 --- a/python/ur_simple_control/managers.py +++ b/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: diff --git a/python/ur_simple_control/util/.get_model.py.swp b/python/ur_simple_control/util/.get_model.py.swp new file mode 100644 index 0000000000000000000000000000000000000000..79d1d380f57f95e5f65f7229101507be08baf750 Binary files /dev/null and b/python/ur_simple_control/util/.get_model.py.swp differ diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py index 7d9cdc2da747113b5db62bb1d50db8e3d0248dac..5dd857ddda653764afb694678c8c3eaf4bfa46bc 100644 --- a/python/ur_simple_control/util/get_model.py +++ b/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