cleaning drc laptop

.gitignore

@@ -60,4 +60,5 @@ target/
 matlab/drake_viewer.out
 
 # Some logs in here are too big
-logs/cf2/vortex0423/Round-2/
+logs/cf2/vortex0423/Round-2/
+vortex/lcmlog-*

lcm/crazyflie_t/__init__.py

@@ -5,6 +5,7 @@
 
 from .crazyflie_pos_t import crazyflie_pos_t
 from .crazyflie_input_t import crazyflie_input_t
+from .crazyflie_positioninput_t import crazyflie_positioninput_t
 from .crazyflie_state_estimate_t import crazyflie_state_estimate_t
 from .crazyflie_imu_t import crazyflie_imu_t
 from .crazyflie_state_estimator_commands_t import crazyflie_state_estimator_commands_t

lcm/crazyflie_t/crazyflie_positioninput_t.py

@@ -0,0 +1,60 @@
+"""LCM type definitions
+This file automatically generated by lcm.
+DO NOT MODIFY BY HAND!!!!
+"""
+
+try:
+    import cStringIO.StringIO as BytesIO
+except ImportError:
+    from io import BytesIO
+import struct
+
+class crazyflie_positioninput_t(object):
+    __slots__ = ["timestamp", "input"]
+
+    def __init__(self):
+        self.timestamp = 0
+        self.input = [ 0.0 for dim0 in range(7) ]
+
+    def encode(self):
+        buf = BytesIO()
+        buf.write(crazyflie_positioninput_t._get_packed_fingerprint())
+        self._encode_one(buf)
+        return buf.getvalue()
+
+    def _encode_one(self, buf):
+        buf.write(struct.pack(">q", self.timestamp))
+        buf.write(struct.pack('>7d', *self.input[:7]))
+
+    def decode(data):
+        if hasattr(data, 'read'):
+            buf = data
+        else:
+            buf = BytesIO(data)
+        if buf.read(8) != crazyflie_positioninput_t._get_packed_fingerprint():
+            raise ValueError("Decode error")
+        return crazyflie_positioninput_t._decode_one(buf)
+    decode = staticmethod(decode)
+
+    def _decode_one(buf):
+        self = crazyflie_positioninput_t()
+        self.timestamp = struct.unpack(">q", buf.read(8))[0]
+        self.input = struct.unpack('>7d', buf.read(56))
+        return self
+    _decode_one = staticmethod(_decode_one)
+
+    _hash = None
+    def _get_hash_recursive(parents):
+        if crazyflie_positioninput_t in parents: return 0
+        tmphash = (0x3fafe4f164026262) & 0xffffffffffffffff
+        tmphash  = (((tmphash<<1)&0xffffffffffffffff)  + (tmphash>>63)) & 0xffffffffffffffff
+        return tmphash
+    _get_hash_recursive = staticmethod(_get_hash_recursive)
+    _packed_fingerprint = None
+
+    def _get_packed_fingerprint():
+        if crazyflie_positioninput_t._packed_fingerprint is None:
+            crazyflie_positioninput_t._packed_fingerprint = struct.pack(">Q", crazyflie_positioninput_t._get_hash_recursive([]))
+        return crazyflie_positioninput_t._packed_fingerprint
+    _get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
+

matlab/Crazyflie.m

@@ -219,12 +219,12 @@ function runController(obj, controller)
       end
 
       position_model = CrazyflieModel();
-      Q = diag([100 100 100 1 1 100 .001 .001 .001 .001 .001 .001]);
+      Q = diag([350 350 1400 5 5 1000 .001 .001 .001 .001 .001 .001]);
       R = eye(7);
       u0 = [0 0 0 0 0 0 position_model.nominal_thrust]';
       options.angle_flag = [0 0 0 1 1 1 0 0 0 0 0 0]';
       [controller,V] = tilqr(position_model,xd,u0,Q,R,options);
-
+         
       state_estimator_frame = LCMCoordinateFrame('crazyflie_state_estimate',StateEstimatesCoder,'x');
       state_estimator_frame.addTransform(AffineTransform(state_estimator_frame,controller.getInputFrame,eye(length(xd)),-xd));
       controller = controller.inInputFrame(state_estimator_frame);
@@ -236,11 +236,38 @@ function runController(obj, controller)
 
     function controller = getPositionControlTvlqr(obj, xtraj, utraj)
 
-      Q = diag([100 100 100 1 1 100 .001 .001 .001 .001 .001 .001]);
+      % forest
+      %Q = diag([80 80 100 1 1 100 .001 .001 .001 .001 .001 .001]);
+
+      % pipes
+      %Q = diag([100 100 100 1 1 100 .001 .001 .001 .001 .001 .001]);
+
+      % walls
+      %Q = diag([120 120 150 1 1 100 .001 .001 .001 .001 .001 .001]);
+
+      % strings
+      %Q = diag([120 120 120 1 1 150 .001 .001 .001 .001 .001 5]);
+      %Q = diag([450 450 350 10 10 500 .001 .001 .001 .001 .001 10]);
+      %Q = diag([150 150 150 1 1 500 .001 .001 .001 .001 .001 1]);
+      %Q = diag([200 200 200 1 1 300 .001 .001 .001 .001 .001 5]);
+      %Q = diag([120 120 120 1 1 150 .001 .001 .001 .001 .001 5]);
+      %Q = diag([200 200 200 1 1 200 .001 .001 .001 .001 .001 5]);
+
+      Q = diag([300 300 300 2.5 2.5 300 .001 .001 .001 .001 .001 5]);
+      %Q = diag([500 500 500 2.5 2.5 800 .001 .001 .001 .001 .001 5]);
+
       R = eye(7);
-      Qf = Q;
 
       model = CrazyflieModel();
+
+      Qf = Q;
+      %Qti = diag([100 100 100 1 1 100 .001 .001 .001 .001 .001 .001]);
+      %Rti = eye(7);
+      %u0ti = [0 0 0 0 0 0 model.nominal_thrust]';
+      %options.angle_flag = [0 0 0 1 1 1 0 0 0 0 0 0]';
+      %V = tilqr(model,xtraj.eval(xtraj.tspan(2)),u0ti,Qti,Rti,options);
+      %Qf = diag([150 150 150 1 1 300 .001 .001 .001 .001 .001 .001]);
+
       xtraj = xtraj.setOutputFrame(model.getStateFrame);      
       utraj = utraj.setOutputFrame(model.getInputFrame);
 

matlab/crazyflie.urdf

@@ -20,7 +20,7 @@
 
   <link name="base_link">
     <inertial>
-      <mass value="0.03352"/>
+      <mass value="0.0338"/>
       <origin xyz="0 0 0"/>
       <inertia ixx="2.395100000000000e-05" ixy="0" ixz="0" iyy="2.395100000000000e-05" iyz="0" izz="3.234660000000000e-05"/>
     </inertial>

matlab/runtvlqr.m

@@ -1,4 +1,5 @@
 clear cf;
 cf = Crazyflie();
-ctvlqr = cf.getTvlqr(xtraj,utraj,false);
+%ctvlqr = cf.getTvlqr(xtraj,utraj,false);
+ctvlqr = cf.getPositionControlTvlqr(xtraj,utraj);
 cf.runController(ctvlqr);