Merge pull request #28 from RobotLocomotion/lm-bug-fixes

plotting updates and bug fixes

software/config/atlas/common_components.cfg

@@ -236,6 +236,7 @@ network
         "control", "radio", "ROBOT_MODEL", "", 0,
         "control", "radio", "EST_ROBOT_STATE", "", 60,
         "control", "radio", "CONTROLLER_STATE", "", 60,
+        "control", "radio", "POSE_BDI", "", 60,
         "control", "radio", "QP_CONTROLLER_INPUT", "", 60,
         "control", "radio", "ATLAS_COMMAND", "", 60,
         "control", "radio", "FALL_STATE", "", 100,

software/config/atlas/robot.pmd

@@ -73,12 +73,6 @@ group "02.plan_and_control" {
         stop_signal = 15;
     }
 
-    cmd "InstQP on localhost" {
-        exec = "runDRCInstQP -u ${DRC_BASE}/software/drake/drake/examples/Atlas/urdf/atlas_minimal_contact.urdf -c ${DRC_BASE}/software/config/atlas/control_config_hardware.yaml -um ${DRC_BASE}/software/drake/drake/examples/Atlas/config/urdf_modifications_no_hands.yaml -lc ATLAS_COMMAND -lb ATLAS_BEHAVIOR_COMMAND -pub";
-        host = "localhost";
-        stop_signal = 15;
-    }
-
     cmd "PressureControl" {
         exec = "bash -c 'LCM_DEFAULT_URL=${LCM_URL_DRC_ATLAS_1_2} drc-atlas-pressure-monitor'";
         host = "atlas1";

software/config/signal_scope/atlas/l_leg.py

@@ -9,13 +9,14 @@
 
 
 left_leg_joints = ['l_leg_hpz','l_leg_hpx','l_leg_hpy','l_leg_kny','l_leg_aky','l_leg_akx']
+left_leg_joints = ['l_leg_hpz','l_leg_kny','l_leg_aky', 'back_bkz', 'r_leg_aky']
 
 joints = left_leg_joints
 
 # string arrays for EST_ROBOT_STATE and ATLAS_COMMAND
 jn = msg.joint_name
 jns = msg.joint_names
-time_window = 2
+time_window = 5
 
 N = len(joints)
 HSV_tuples =      [(0., 1.0, 1.0),(0.15, 1.0, 1.0), (0.3, 1.0, 1.0), (0.45, 1.0, 1.0), (0.6, 1.0, 1.0), (0.75, 1.0, 1.0), (0.9, 1.0, 1.0)]
@@ -33,8 +34,10 @@
 # velocity plot
 addPlot(timeWindow=time_window, yLimits=[-2, 2])
 
-addSignals('EST_ROBOT_STATE', msg.utime, msg.joint_position, joints, keyLookup=jn, colors=RGB_tuples_v3)
+addSignals('EST_ROBOT_STATE', msg.utime, msg.joint_velocity, joints, keyLookup=jn, colors=RGB_tuples_v3)
+addSignals('CONTROLLER_STATE', msg.timestamp, msg.vref_integrator_state, joints, keyLookup=jn, colors=RGB_tuples)
 
+# addSignals('CORE_ROBOT_STATE', msg.utime, msg.joint_velocity, joints, keyLookup=jn, colors=RGB_tuples_dark)
 
 # effort plot
 addPlot(timeWindow=time_window, yLimits=[-200,200])
@@ -44,7 +47,9 @@
 addSignals('EST_ROBOT_STATE', msg.utime, msg.joint_effort, joints, keyLookup=jn, colors=RGB_tuples_v3)
 
 
-# addSignals('ATLAS_COMMAND', msg.utime, msg.effort, joints, keyLookup=jns, colors=RGB_tuples_v3)
+# addSignals('CORE_ROBOT_STATE', msg.utime, msg.joint_effort, joints, keyLookup=jn, colors=RGB_tuples_dark)
+
+addSignals('ATLAS_COMMAND', msg.utime, msg.effort, joints, keyLookup=jns, colors=RGB_tuples)
 
 
 

software/config/signal_scope/atlas/pelvis_pose.py

@@ -42,24 +42,48 @@ def myFunctionBDI(msg):
 
 
 
+def poseY(msg):
+	rotation = msg.pose.rotation
+	quat = numpy.array([rotation.w, rotation.x, rotation.y, rotation.z])
+	rpy = transformUtils.quaternionToRollPitchYaw(quat)
+	return msg.utime, rpy[1]
+
+def poseYBDI(msg):
+	quat = numpy.array([msg.orientation[0], msg.orientation[1], msg.orientation[2], msg.orientation[3]])
+	rpy = transformUtils.quaternionToRollPitchYaw(quat)
+	return msg.utime, rpy[1]
+
+
+
 
 # position plot
-addPlot(timeWindow=7, yLimits=[-1.5, 1.5])
+addPlot(timeWindow=5, yLimits=[-1.5, 1.5])
 
 addSignalFunction('EST_ROBOT_STATE', myFunction)
-addSignalFunction('EST_ROBOT_STATE_1', myFunction)
+# addSignalFunction('EST_ROBOT_STATE_1', myFunction)
 addSignalFunction('EST_ROBOT_STATE_ORIGINAL', myFunction)
 addSignalFunction('POSE_BODY', myFunctionBDI)
 addSignalFunction('POSE_BDI', myFunctionBDI)
 
+addSignalFunction('EST_ROBOT_STATE', poseY)
+addSignalFunction('POSE_BDI', poseYBDI)
+
 
 # velocity plot
-addPlot(timeWindow=7, yLimits=[-1.5, 1.5])
+addPlot(timeWindow=5, yLimits=[-1.5, 1.5])
 addSignal('POSE_BODY', msg.utime, msg.rotation_rate[2])
 addSignal('POSE_BODY_ORIGINAL', msg.utime, msg.rotation_rate[2])
+
+addSignal('EST_ROBOT_STATE', msg.utime, msg.twist.angular_velocity.z)
 addSignal('POSE_BDI', msg.utime, msg.rotation_rate[2])
 
 
+addSignal('EST_ROBOT_STATE', msg.utime, msg.twist.angular_velocity.y)
+addSignal('POSE_BDI', msg.utime, msg.rotation_rate[1])
+
+addPlot(timeWindow=5, yLimits=[-1.5, 1.5])
+addSignal('EST_ROBOT_STATE', msg.utime, msg.twist.linear_velocity.x)
+addSignal('POSE_BDI', msg.utime, msg.vel[0])
 
 
 

software/config/signal_scope/atlas/qp_info.py

@@ -0,0 +1,68 @@
+
+
+import numpy
+import colorsys
+from director import transformUtils
+
+# joints to plot
+joints = ["rightHipYaw", "rightHipRoll", "rightHipPitch", "rightKneePitch", "rightAnklePitch", "rightAnkleRoll"]
+
+left_arm_joints = ['l_arm_shz','l_arm_shx','l_arm_ely','l_arm_elx','l_arm_uwy',
+  'l_arm_mwx', 'l_arm_lwy']
+
+
+joints = left_arm_joints
+
+joints = ['l_arm_shx']
+# string arrays for EST_ROBOT_STATE and ATLAS_COMMAND
+jn = msg.joint_name
+jns = msg.joint_names
+
+N = len(joints)
+HSV_tuples =      [(0., 1.0, 1.0),(0.15, 1.0, 1.0), (0.3, 1.0, 1.0), (0.45, 1.0, 1.0), (0.6, 1.0, 1.0), (0.75, 1.0, 1.0), (0.9, 1.0, 1.0)]
+HSV_tuples_dark = [(0., 1.0, 0.5),(0.15, 1.0, 0.5), (0.3, 1.0, 0.5), (0.45, 1.0, 0.5), (0.6, 1.0, 0.5), (0.75, 1.0, 0.5), (0.9, 1.0, 0.5)]
+HSV_tuples_v3 = [(0.,0.75, 0.5),(0.15,0.75, 0.5), (0.3,0.75, 0.5), (0.45,0.75, 0.5), (0.6,0.75, 0.5), (0.75,0.75, 0.5), (0.9,0.75, 0.5)]
+
+HSV_tuples =      [(0.15, 1.0, 1.0), (0.3, 1.0, 1.0), (0.45, 1.0, 1.0), (0.6, 1.0, 1.0), (0.75, 1.0, 1.0), (0.9, 1.0, 1.0)]
+HSV_tuples_dark = [(0., 1.0, 0.5),(0.15, 1.0, 0.5), (0.3, 1.0, 0.5), (0.45, 1.0, 0.5), (0.6, 1.0, 0.5), (0.75, 1.0, 0.5), (0.9, 1.0, 0.5)]
+HSV_tuples_v3 = [(0.3,0.75, 0.5), (0.45,0.75, 0.5), (0.6,0.75, 0.5), (0.75,0.75, 0.5), (0.9,0.75, 0.5)]
+
+RGB_tuples = map(lambda x: colorsys.hsv_to_rgb(*x), HSV_tuples)
+RGB_tuples_dark = map(lambda x: colorsys.hsv_to_rgb(*x), HSV_tuples_dark)
+RGB_tuples_v3 = map(lambda x: colorsys.hsv_to_rgb(*x), HSV_tuples_v3)
+
+def myFunction(msg):
+	rotation = msg.pose.rotation
+	quat = numpy.array([rotation.w, rotation.x, rotation.y, rotation.z])
+	rpy = transformUtils.quaternionToRollPitchYaw(quat)
+	return msg.utime, rpy[2]
+
+def myFunctionBDI(msg):
+	quat = numpy.array([msg.orientation[0], msg.orientation[1], msg.orientation[2], msg.orientation[3]])
+	rpy = transformUtils.quaternionToRollPitchYaw(quat)
+	return msg.utime, rpy[2]
+
+
+def plotUtime(msg):
+	return msg.utime/1e6
+
+def plotTimestamp(msg):
+	return msg.timestamp/1e6
+
+
+
+
+# position plot
+addPlot(timeWindow=5, yLimits=[-1.5, 1.5])
+
+addSignal('FOOT_CONTACT_ESTIMATE', msg.utime, msg.left_contact)
+addSignal('FOOT_CONTACT_ESTIMATE', msg.utime, msg.right_contact)
+
+
+addPlot(timeWindow=5, yLimits=[-1.5, 1.5])
+addSignal('CONTROLLER_STATE', msg.timestamp, msg.qpInfo)
+
+
+addPlot(timeWindow=5, yLimits=[0, 1500])
+addSignal('EST_ROBOT_STATE', msg.utime, msg.force_torque.l_foot_force_z)
+addSignal('EST_ROBOT_STATE', msg.utime, msg.force_torque.r_foot_force_z)