diff --git a/python/examples/smc_node.py b/python/examples/smc_node.py
index 7fc29a86c4eb2704004a979e7cd7ef544bdb6404..6d55386f0a0cb9f736d122bba4b4a55ccac84372 100644
--- a/python/examples/smc_node.py
+++ b/python/examples/smc_node.py
@@ -2,12 +2,15 @@
import rclpy
from rclpy.node import Node
from geometry_msgs import msg
+from nav_msgs.msg import Odometry
from rclpy import wait_for_message
import pinocchio as pin
import argcomplete, argparse
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, controlLoopCompliantClik, invKinmQP
+from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
+from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoIKMPC
import threading
@@ -16,14 +19,15 @@ def get_args():
parser.description = 'Run closed loop inverse kinematics \
of various kinds. Make sure you know what the goal is before you run!'
parser = getClikArgs(parser)
+ parser = get_OCP_args(parser)
argcomplete.autocomplete(parser)
args = parser.parse_args()
return args
-class ROSCommHandler(Node):
+class ROSCommHandlerHeron(Node):
def __init__(self, args, robot_manager : RobotManager, loop_manager : ControlLoopManager):
- super().__init__('ROSCommHandler')
+ super().__init__('ROSCommHandlerHeron')
self.publisher_vel_base = self.create_publisher(msg.Twist, '/cmd_vel', 5)
print("created publisher")
@@ -35,20 +39,29 @@ class ROSCommHandler(Node):
history = rclpy.qos.HistoryPolicy.KEEP_LAST,
depth = 1,
)
- self.subscription = self.create_subscription(msg.PoseWithCovarianceStamped,
+# MASSIVE TODO: you want to be subsribed to a proper localization thing,
+# specifically to output from robot_localization
+ self.subscription_amcl = self.create_subscription(msg.PoseWithCovarianceStamped,
'/amcl_pose', self.pose_callback, qos_prof)
+ self.subscription_odom = self.create_subscription(Odometry,
+ '/odom', self.odom_callback, qos_prof)
#wait_for_message.wait_for_message(msg.PoseWithCovarianceStamped, self, '/amcl_pose')
#print("subscribed to /amcl_pose")
- self.subscription # prevent unused variable warning
+ self.subscription_amcl # prevent unused variable warning
+ self.subscription_odom # prevent unused variable warning
self.robot_manager = robot_manager
self.loop_manager = loop_manager
self.args = args
+ # ONE TRILLION PERCENT YOU NEED TO INTEGRATE/USE ODOM IN-BETWEEN THESE
def pose_callback(self, mesg):
self.robot_manager.q[0] = mesg.pose.pose.position.x
self.robot_manager.q[1] = mesg.pose.pose.position.y
# TODO: check that z can be used as cos(theta) and w as sin(theta)
# (they could be defined some other way or theta could be offset of something)
+ # ONE TRILLION PERCENT THIS ISN'T CORRECT
+ # TODO: generate a rotation matrix from this quarternion with pinocchio,
+ # and give this is the value of the x or the y axis
self.robot_manager.q[2] = mesg.pose.pose.orientation.z
self.robot_manager.q[3] = mesg.pose.pose.orientation.w
@@ -56,6 +69,17 @@ class ROSCommHandler(Node):
print("received new amcl")
#self.publisher_vel_base.publish(vel_cmd)
+ # HIGHLY LIKELY THAT THIS IS NOT THE THE VELOCITY OF THE BASE FRAME
+ def odom_callback(self, mesg : Odometry):
+ self.robot_manager.v_q[0] = mesg.twist.twist.linear.x
+ self.robot_manager.v_q[1] = mesg.twist.twist.linear.y
+ # TODO: check that z can be used as cos(theta) and w as sin(theta)
+ # (they could be defined some other way or theta could be offset of something)
+ self.robot_manager.v_q[2] = mesg.twist.twist.angular.z
+
+ #vel_cmd = msg.Twist()
+ print("received new amcl")
+ #self.publisher_vel_base.publish(vel_cmd)
@@ -65,8 +89,9 @@ if __name__ == '__main__':
robot = RobotManager(args)
goal = robot.defineGoalPointCLI()
#goal = pin.SE3.Identity()
- loop_manager = compliantMoveL(args, robot, goal, run=False)
- ros_comm_handler = ROSCommHandler(args, robot, loop_manager)
+ #loop_manager = compliantMoveL(args, robot, goal, run=False)
+ loop_manager = CrocoIKMPC(args, robot, goal, run=False)
+ ros_comm_handler = ROSCommHandlerHeron(args, robot, loop_manager)
thread = threading.Thread(target=rclpy.spin, args=(ros_comm_handler,), daemon=True)
thread.start()
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index 9450f01b9ff3639106a03cef0cc1522b1ebe61c2..3b476c272538d9a1f12c522c5ae7d32bfa054cde 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc differ
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index df733acfd49f789012812efcd66287fff25aeee7..9a943bc19503f8e94bbba1ce9b3d060f2e0a2498 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -13,7 +13,7 @@ from ur_simple_control.util.grippers.robotiq.robotiq_gripper import RobotiqGripp
from ur_simple_control.util.grippers.on_robot.twofg import TWOFG
import copy
import signal
-from ur_simple_control.util.get_model import get_model, heron_approximation, heron_approximationDD, getGripperlessUR5e
+from ur_simple_control.util.get_model import get_model, heron_approximation, heron_approximationDD, getGripperlessUR5e, mir_approximation
from collections import deque
from ur_simple_control.visualize.visualize import plotFromDict, realTimePlotter, manipulatorVisualizer
from ur_simple_control.util.logging_utils import LogManager
@@ -86,7 +86,7 @@ def getMinimalArgParser():
#################################################
parser.add_argument('--robot', type=str, \
help="which robot you're running or simulating", default="ur5e", \
- choices=['ur5e', 'heron', 'heronros', 'gripperlessur5e'])
+ choices=['ur5e', 'heron', 'heronros', 'gripperlessur5e', 'mirros'])
parser.add_argument('--simulation', action=argparse.BooleanOptionalAction, \
help="whether you are running the UR simulator", default=False)
parser.add_argument('--robot-ip', type=str,
@@ -424,6 +424,9 @@ class RobotManager:
if self.robot_name == "heronros":
self.model, self.collision_model, self.visual_model, self.data = \
heron_approximation()
+ if self.robot_name == "mirros":
+ self.model, self.collision_model, self.visual_model, self.data = \
+ mir_approximation()
#self.publisher_vel_base = create_publisher(msg.Twist, '/cmd_vel', 5)
#self.publisher_vel_base = publisher_vel_base
if self.robot_name == "gripperlessur5e":
@@ -494,6 +497,10 @@ class RobotManager:
# so it can't be all zeros
if self.robot_name == "heron":
self.q[2] = 1.0
+ if self.robot_name == "heronros":
+ self.q[2] = 1.0
+ if self.robot_name == "mirros":
+ self.q[2] = 1.0
# v_q is the generalization of qd for every type of joint.
# for revolute joints it's qd, but for ex. the planar joint it's the body velocity.
self.v_q = np.zeros(self.model.nv)
@@ -836,13 +843,35 @@ class RobotManager:
self.v_q = qd
self.q = pin.integrate(self.model, self.q, qd * self.dt)
- # TODO: implement real thing
if self.robot_name == "heron":
# y-direction is not possible on heron
qd[1] = 0
self.q = pin.integrate(self.model, self.q, qd * self.dt)
+
if self.robot_name == "heronros":
# y-direction is not possible on heron
+ # TODO: REMOVE OBVIOUSLY
+#############################################33
+# qd[:] = 0.0
+#############################################33
+
+ qd[1] = 0
+ cmd_msg = msg.Twist()
+ cmd_msg.linear.x = qd[0]
+ cmd_msg.angular.z = qd[2]
+ #print("about to publish", cmd_msg)
+ self.publisher_vel_base.publish(cmd_msg)
+ # good to keep because updating is slow otherwise
+ # it's not correct, but it's more correct than not updating
+ #self.q = pin.integrate(self.model, self.q, qd * self.dt)
+
+ if self.robot_name == "mirros":
+ # y-direction is not possible on heron
+ # TODO: REMOVE OBVIOUSLY
+#############################################33
+ #qd[:] = 0.0
+#############################################33
+
qd[1] = 0
cmd_msg = msg.Twist()
cmd_msg.linear.x = qd[0]
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 644615345770826ac408fc3975adc19eebec6fdc..9f5debcc60a06eaaaf2da42a971e6768a59ef75d 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -60,7 +60,7 @@ def CrocoIKMPCControlLoop(args, robot : RobotManager, solver, i, past_data):
return breakFlag, save_past_dict, log_item
-def CrocoIKMPC(args, robot, goal):
+def CrocoIKMPC(args, robot, goal, run=True):
"""
IKMPC
-----
@@ -92,7 +92,10 @@ def CrocoIKMPC(args, robot, goal):
}
save_past_dict = {}
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
- loop_manager.run()
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, i, past_data):
"""
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 09157b30ead2d179ff9301f477cbc1fb5dec347a..2da5229203ef2f6a7fda2a966ba14d5418724cbd 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -104,7 +104,7 @@ def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
# and this should be 2 * model.nv the way things are defined here.
- if robot.robot_name == "heron":
+ if robot.robot_name == "heron" or robot.robot_name == "heronros" or robot.robot_name == "mirros":
xlb = xlb[1:]
xub = xub[1:]
@@ -293,7 +293,7 @@ def createCrocoEEPathFollowingOCP(args, robot : RobotManager, x0):
# from one to point to the other.
# point activation input and the residual need to be of the same length obviously,
# and this should be 2 * model.nv the way things are defined here.
- if robot.robot_name == "heron":
+ if robot.robot_name == "heron" or robot.robot_name == "heronros" or robot.robot_name == "mirros":
xlb = xlb[1:]
xub = xub[1:]
@@ -447,7 +447,7 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
# and this should be 2 * model.nv the way things are defined here.
- if robot.robot_name == "heron":
+ if robot.robot_name == "heron" or robot.robot_name == "heronros" or robot.robot_name == "mirros":
xlb = xlb[1:]
xub = xub[1:]
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc
index f3163af30f2c40fddc884a5ec95aabd5f6cbf222..7d915c240c73fce908dbaf592187296265279892 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-310.pyc differ
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 71fcdc3f7dba9fb6cfec27070b67852c3b068c22..f687b61f674f7f8eace4fed4d9408900a88ad4b8 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -235,6 +235,64 @@ def heron_approximation():
return model, visual_model.copy(), visual_model, data
+
+def mir_approximation():
+ # 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.
+ # TODO: put in heron's values
+ # TODO: make these parameters the same as in mpc_params in the planner
+ model_mobile_base.velocityLimit[0] = 2
+ model_mobile_base.velocityLimit[1] = 0
+ model_mobile_base.velocityLimit[2] = 2
+ # TODO: i have literally no idea what reasonable numbers are here
+ model_mobile_base.effortLimit[0] = 200
+ model_mobile_base.effortLimit[1] = 0
+ model_mobile_base.effortLimit[2] = 200
+ #print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
+ #body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
+ # box_dimensions[1], box_dimensions[2])
+
+ # 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
+ # it's tool0 because that's used everywhere
+ model_mobile_base.addFrame(pin.Frame('tool0',MOBILE_BASE_JOINT_ID,0,joint_placement.copy(),pin.FrameType.JOINT) )
+
+ data = model_mobile_base.createData()
+
+ return model_mobile_base, geom_model_mobile_base.copy(), geom_model_mobile_base.copy(), data
+
# TODO:
# try building the mobile base as a rotational joint,
# on top of which is a prismatic joint.