[ return ]

genmcx



#!/usr/usc/bin/perl
#
#  usage: genmcx [-]* 
#
#
#  Read network specification files for mcx and F5 and 
# determine the set of connections between them.
#
#  This is done outside of mknet because of the significant computational
# cost.
#

require "mknet.defs.pl";
#require "parms.pl";
#require "distributed.pl";
#require "object.pl";
#require "connect.pl";
require "parse.pl";
#require "filter.pl";
require "support.pl";
#require "dump.pl";
require "matlab.pl";
require "f5.pl";
require "mcx.pl";
require "grasp.pl";
require "flush.pl";
require "log.pl";

				# Parameters
$parms = ();
$parms{"d"} = 0;		# Debug mask

				# Database initialization
%objects = ();
%net = ();
%f5 = ();
%aip = ();
%grasp = ();
%ogmap = ();
%mcx = ();


#
#  sub interpret_parms
#
#  Interpret the parameters and stuff them into the %parms
# structure.
#


sub interpret_parms
{
    local($i);

    foreach $i (@ARGV[0..$#ARGV-1]) # Loop through each parm
    {
	if($i =~ /-(\w)(.*)/)	# Expect a - where  is a single 
	{			#  letter.
	    $parms{$1} = $2;
#	    print "$1 = $2\n";
	}
	else			# Not the right format.
	{
	    print "Parameter Error: \"$i\"\n";
	    return 0;
	};
    };
    return 1;
};
############################################################
#  
#

sub init_data_structures
{
    local($i);

				# Set number of columns in each layer to 0 
    foreach $i (@LAYERS)
    {
	$net{"num", $i} = 0;
    }
};
############################################################

#
# sub null_weights
#
#  Set all mcx weights to 0
#
#

sub null_weights
{
    local($i) = @_;

    $f5{"weights", "p"} = "0:" x $mcx{"num", "position"};
    $f5{"weights", "f"} = "0:" x $mcx{"num", "force"};
    $f5{"weights", "if"} = "0:" x $mcx{"num", "inverse force"};
};


#
#  sub joint_index_to_tip_pad
#
#  Map joint index into a tip pad.
#

sub joint_index_to_tip_pad
{
    local($joint_index) = @_;
    local($joint) = $JOINTS[$joint_index];

    $joint eq "t0" && return("");
    &member($joint, ("t1", "t2")) && return("t0");
    &member($joint, ("i0", "i1", "i2")) && return("i2");
    &member($joint, ("m0", "m1", "m2")) && return("m2");
    &member($joint, ("r0", "r1", "r2")) && return("r2");
    &member($joint, ("l0", "l1", "l2")) && return("l2");

    die "joint_index_to_tip_pad(): bad joint index specified ($joint_index)\n";
};

#  
#  sub joint_mapped_p
#
#  Return a 1 iff the joint maps to a finger pad and we are expecting
# input from that finger pad.  The first bit of info comes from the 
# assumption that we are doing a precision grasp or a side opposition.
# (and not a power grasp).  The second piece of info comes from
# the tactile mask for the grasp
#

sub joint_mapped_p
{
    local($joint, $grasp_id) = @_;

    local($pad) = &joint_index_to_tip_pad($joint);

				# Does a mapping exist for this joint?
    if($pad eq "")
    {
	return(0);
    }
    else			# Yes, then check the mask
    {
	print "TACTILE mask: $grasp_id $joint $pad " . $grasp{"tactile mask", $grasp_id, $pad} . "\n";
	return($grasp{"tactile mask", $grasp_id, $pad});
    };
};

#
#  sub generate_response_levels
#
#  Decides whether a particular finger is going to be driven by an
# f5 unit.  Keeps rolling until at least one finger is driven, but not
# more than 3 (we start having trouble finding solutions with grad.m 
# if all 5 fingers are allowed to be driven).
#  Also, if $force_flag is set to 1, then this means that we must also
# filter out joints that should not respond to the force/inverse-force
# signals.
#  If $force_flag is set to 2, then we are trying to 
# compute position weights for those joints that will not be driven
# by a reflex.

sub generate_response_levels
{
    local($force_flag, $grasp_id) = @_;
    local($i);
    local(@outputs) = ();
    local($flag) = 1;
    local($count);
    local($j0, $j1, $j2);
#    print "#############################################\n";
#    print "$force_flag $grasp_id\n";
				# Loop as many times as necessary
    while($flag)
    {
	@outputs = ();
	$count = 0;
				# Scan for each finger
				#  (ie allocate in groups).
	for($i = 0; $i < ($#JOINTS+1)/3; ++$i)
	{
	    print "FINGER $i\n";
				# Roll the die
	    if(rand() < $F5_FINGER_PROB)
	    {			# This finger should receive inputs

				# If this is a force operation, then
				#   need to check if the joints should
				#   respond to a force input
				#  We've assumed here that on a finger
				#  joints 0,1,and 2 are presented in that
				#  order in the JOINTS list.

				# Default - specify all joints
		$j0 = $j1 = $j2 = 1;
		if($force_flag == 1)
		{
				# Filter out joints that should not be
				#  driven by reflexes
		    $j0 = &joint_mapped_p($i*3, $grasp_id);
		    $j1 = &joint_mapped_p($i*3+1, $grasp_id);
		    $j2 = &joint_mapped_p($i*3+2, $grasp_id);
		}
		elsif($force_flag == 2)
		{
				# Filter out joints that are
				#  driven by reflexes.
		    $j0 = !&joint_mapped_p($i*3, $grasp_id);
		    $j1 = !&joint_mapped_p($i*3+1, $grasp_id);
		    $j2 = !&joint_mapped_p($i*3+2, $grasp_id);
		};
		print "Generate RL: filtering $j0 $j1 $j2\n";
		push(@outputs, ($j0, $j1, $j2));

				# Only count this finger as participating
				#  if at least one joint has been allocated.
		if($j0 || $j1 || $j2)
		{
		    if(!$j0 || !$j1 || !$j2)
		    {
			print "Generate RL: filtering $j0 $j1 $j2\n";
		    };
		    ++$count;
		};
	    }
	    else
	    {
		push(@outputs, (0,0,0));
	    };
	}
				# Terminate loop only if at least one finger
				#  has been selected and not more than 3.
	$flag = ($count == 0 || $count > 3);
    };
    print "done $count\n";
    return(@outputs);
};


#
#  sub compute_extension_weights
#
#  Compute mcx weights for extension phase of movement.
# Connect only to position units.
#
#

sub compute_extension_weights
{
    local($base, $i) = @_;
    local(@jnts, @response);

				# Grasp id of most responsive grasp
    local($grasp_id) =
	&translate_grasp_parms_2_id($f5{"grasp", $i, $f5{"primary grasp", $i}},
				    $f5{"grasp aperture", $i,
				    $f5{"primary grasp", $i}});


				# Get the joint goals
    @jnts = &fetch_joint_vector($i, "preshape pos");
#    print "JOINT: " . $jnts[0] . "\n";
				# Randomly generate a response vector
    @response =
	&generate_response_levels(0, $grasp_id);

				# Compute a set of weights that
				#  satisfies the goal

    $f5{"weights", "p", $i} =
	&compute_weight_vector($base, $i, "p", "position",
			       $f5{"grasp response", $i,
				       $f5{"primary grasp", $i}}
			       *$f5{"phase response", $i,
				    $f5{"primary grasp", $i},
				    $f5{"primary phase", $i}}
			       * $F5_MCX_GAIN,
			       @jnts, @response);

				# Weights to force and inverse force
				#  units are null.
    $f5{"weights", "f", $i} = "0:" x $mcx{"num", "force"};
    $f5{"weights", "if", $i} = "0:" x $mcx{"num", "inverse force"};
};


#
#  sub add_vecs
#
#  Add two vectors and multiply the result with a gain
#
#

sub add_vecs
{
    local($gain, @rest) = @_;
				# First half of 
    local(@v1) = @rest[0..($#rest-1)/2];
				# Second half
    local(@v2) = @rest[($#rest+1)/2..$#rest];
    local(@out) = ();
    local($i);
#    print "add_vecs\n";
    for($i = 0; $i <= $#v1; ++$i)
    {
#	print $v1[$i] . " " . $sign * $v2[$i] . " " .  $gain . "\n";
	push(@out,  ($v1[$i] + $gain * $v2[$i]));
    };
    return(@out);
};


#
#  sub compute_flexion_weights
#
#  Compute mcx weights for flexion phase of movement.
# Connect to position and inverse force units.
#
#

sub compute_flexion_weights
{
    local($base, $i) = @_;
    local(@jnts, @djoings, @response);

				# Grasp id of most responsive grasp
    local($grasp_id) =
	&translate_grasp_parms_2_id($f5{"grasp", $i, $f5{"primary grasp", $i}},
				    $f5{"grasp aperture", $i,
				    $f5{"primary grasp", $i}});

    &write_log("flexion: final pos\n");
				# Get the joint goals
    @jnts = &fetch_joint_vector($i, "final pos");

				# Randomly generate a response vector
    @response =
	&generate_response_levels(0, $grasp_id);

				# Compute a set of weights that
				#  satisfies the goal

    $f5{"weights", "p", $i} =
	&compute_weight_vector($base, $i, "p", "position",
			       $f5{"grasp response", $i,
				       $f5{"primary grasp", $i}}
			       *$f5{"phase response", $i,
				    $f5{"primary grasp", $i},
				    $f5{"primary phase", $i}}
			       * $F5_MCX_GAIN,
			       @jnts, @response);

				# Weights to force
				#  units are null.
    $f5{"weights", "f", $i} = "0:" x $mcx{"num", "force"};


    &write_log("flexion: inverse force\n");
				# Weights to inverse force.
				# Get the jacobian
    @djnts = &fetch_joint_vector($i, "jacobian");

    @jnts = &add_vecs(-$F5_MCX_J_GAIN, @jnts, @djnts);

				# Randomly generate a response vector
    @response =
	&generate_response_levels(1, $grasp_id);

				# Compute a set of weights that
				#  satisfies the goal


    $f5{"weights", "if", $i} = 
	&compute_weight_vector($base, $i, "if", "inverse force",
			       $f5{"grasp response", $i,
				       $f5{"primary grasp", $i}}
			       *$f5{"phase response", $i,
				    $f5{"primary grasp", $i},
				    $f5{"primary phase", $i}}
			       * $F5_MCX_GAIN * $F5_MCX_FORCE_GAIN,
			       @jnts, @response);
};




#
#  sub compute_hold_weights
#
#  Compute mcx weights for hold phase of movement.
# Connect to force and inverse force units
#
#

sub compute_hold_weights
{
    local($base, $i) = @_;
    local(@jnts, @njnts, @djoings, @response);

#    print "HOLD\n";
				# Grasp id of most responsive grasp
    local($grasp_id) =
	&translate_grasp_parms_2_id($f5{"grasp", $i, $f5{"primary grasp", $i}},
				    $f5{"grasp aperture", $i,
				    $f5{"primary grasp", $i}});

				# Get the joint goals
    @jnts = &fetch_joint_vector($i, "final pos");


				# Position weights

				# Randomly generate a response vector
#    print "HOLD POS\n";
    @response =
	&generate_response_levels(2, $grasp_id);

    $f5{"weights", "p", $i} =
	&compute_weight_vector($base, $i, "p", "position",
			       $f5{"grasp response", $i,
				       $f5{"primary grasp", $i}}
			       *$f5{"phase response", $i,
				    $f5{"primary grasp", $i},
				    $f5{"primary phase", $i}}
			       * $F5_MCX_GAIN,
			       @jnts, @response);




				# Get the jacobian
    @djnts = &fetch_joint_vector($i, "jacobian");

				# Weights to force.
    @njnts = &add_vecs($F5_MCX_J_GAIN, @jnts, @djnts);

				# Randomly generate a response vector
#    print "HOLD FORCE\n";
    @response =
	&generate_response_levels(1, $grasp_id);

				# Compute a set of weights that
				#  satisfies the goal

    $f5{"weights", "f", $i} = 
	&compute_weight_vector($base, $i, "f", "force",
			       $f5{"grasp response", $i,
				       $f5{"primary grasp", $i}}
			       *$f5{"phase response", $i,
				    $f5{"primary grasp", $i},
				    $f5{"primary phase", $i}}
			       * $F5_MCX_GAIN * $F5_MCX_FORCE_GAIN
			         * $F5_MCX_FORCE_GAIN_MOD,
			       @njnts, @response);



				# Weights to inverse force.
    @njnts = &add_vecs(-$F5_MCX_J_GAIN, @jnts, @djnts);

				# Randomly generate a response vector
#    print "HOLD IFORCE\n";
    @response =
	&generate_response_levels(1, $grasp_id);

				# Compute a set of weights that
				#  satisfies the goal

    $f5{"weights", "if", $i} = 
	&compute_weight_vector($base, $i, "if", "inverse force",
			       $f5{"grasp response", $i,
				       $f5{"primary grasp", $i}}
			       *$f5{"phase response", $i,
				    $f5{"primary grasp", $i},
				    $f5{"primary phase", $i}}
			       * $F5_MCX_GAIN * $F5_MCX_FORCE_GAIN,
			       @njnts, @response);
};




#
#  sub compute_release_weights
#
#  Compute mcx weights for release phase of movement.
# Connect only to force units.
#
#

sub compute_release_weights
{
    local($base, $i) = @_;
    local(@jnts, @njnts, @djoings, @response);

				# Grasp id of most responsive grasp
    local($grasp_id) =
	&translate_grasp_parms_2_id($f5{"grasp", $i, $f5{"primary grasp", $i}},
				    $f5{"grasp aperture", $i,
				    $f5{"primary grasp", $i}});

				# Weights to position units
				# Get the joint goals
    @jnts = &fetch_joint_vector($i, "final pos");

				# Randomly generate a response vector

				# Get the jacobian
    @djnts = &fetch_joint_vector($i, "jacobian");

				# Weights to force.
    @njnts = &add_vecs($F5_MCX_J_GAIN, @jnts, @djnts);

				# Randomly generate a response vector
    @response =
	&generate_response_levels(0, $grasp_id);

				# Compute a set of weights that
				#  satisfies the goal

    $f5{"weights", "p", $i} = 
	&compute_weight_vector($base, $i, "p", "position",
			       $f5{"grasp response", $i,
				       $f5{"primary grasp", $i}}
			       *$f5{"phase response", $i,
				    $f5{"primary grasp", $i},
				    $f5{"primary phase", $i}}
			       * $F5_MCX_GAIN,
			       @njnts, @response);

				# Weights to force units

    $f5{"weights", "f", $i} =
	"0:" x $mcx{"num", "force"};


				# Weights to inverse force.
    $f5{"weights", "if", $i} =
	"0:" x $mcx{"num", "inverse force"};

};



#  
#  sub compute_weights
#
#  Compute the weight vector from F5 unit $i to the
# mcx units.
#  For the time being, we assume that the only connections
# we need to make from unit $i are those that serve the
# cell's primary grasp (the grasp to which it responds most 
# highly), and the cell's primary phase (the phase during which
# the cell is responding the most).
#  In reality, we should try to find some common set of connections
# that satisfy all response properties of the neuron, but this is
# a much harder problem.
#
#  An individual mcx cell can be one of three types (position,
# force, and inverse force).  Whether or not an f5 cell connects
# to an mcx cell of these classes depends upon the primary
# phase of the cell.:
#  SET: no connections
#  EXTENSION: position mcx cells, with the goal of the max aperture
#    (as specified in the gspec).
#  FLEXION: mix of position and inverse force, with the goals
#    corresponding to the final grasp, and a position that is
#    internal to the target object (ie final grasp + gain * Jacobian)
#  HOLD: equal mix of inverse force and force, with the goals
#    corresponding to grasp + gain * Jacobian and 
#    grasp - gain * Jacobian.
#  RELEASE: mcx force cells, with the goal of
#    grasp - gain * Jacobian
#
#  Note that f5 cells that are not aperture-specific
# are allowed to have connections.
#
#  For each mcx cell type, we will call the matlab program
# grad.m to compute a set of weights that generate the goal
# position.
#
#

sub compute_weights
{
    local($base, $i) = @_;
    local($pf, $pg);

    $pf = $f5{"primary phase", $i};
    $pg = $f5{"primary grasp", $i};

    print "considering unit $i: $pf\n";
    &write_log("considering unit $i: $pf\n");


				# The cell must be primarily committed
				#  to a single phase.
    if($f5{"phase response", $i, $pg, $pf}/$f5{"total phase response",$i,$pg}
       < $F5_MCX_MIN_RATIO)
    {
	print "Too sparse\n";
	&write_log("Too sparse\n");
	&null_weights($i);
    }
    elsif($f5{"primary phase", $i} eq "set")
    {
	&null_weights($i);
    }
    elsif($f5{"primary phase", $i} eq "extension")
    {
	&compute_extension_weights($base, $i);
    }
    elsif($f5{"primary phase", $i} eq "flexion")
    {
	&compute_flexion_weights($base, $i);
    }
    elsif($f5{"primary phase", $i} eq "hold")
    {
	&compute_hold_weights($base, $i);
    }
    elsif($f5{"primary phase", $i} eq "release")
    {
	&compute_release_weights($base, $i);
    }
};


############################################################

sub loop_f5
{
    local($base) = @_;
    local($i);
				# Loop over each f5 unit
    for($i = 0; $i < $f5{"num"}; ++$i)
    {
	&write_log("Analyzing F5 unit $i\n");

	&analyze_f5_unit($i);	# Compute primary grasp

				# If the primary grasp has
				#  a prefered aperture, then
				#  compute the weights
	if($f5{"grasp aperture", $i, $f5{"primary grasp", $i}} != -1)
	{
	    &compute_weights($base, $i);
	}
	else
	{
	    &null_weights($i);
	};
#	if($i == 10)
#	{
#	    die "END\n";
#	};
    };
};

#
#  sub dump_f5_weights
#
#  Write out weights to a temporary file (to be picked up by mknet).
#

sub dump_f5_weights
{
    local($i, $j);
    local($fname) = $base . ".f5.mcx.w";
    local(@wp, @wf, @wif);
    local($tmp);
    local($type, $ind, $val);

    open(FP, ">$fname") || die "dump_weights(): error opening output file ($fname): $!.\n";

    print FP "f5.mcx.w\n";
    for($i = 0; $i < $f5{"num"}; ++$i)
    {
	print FP "F5.$i\n";

				#  Expand weight vectors of different
				# mcx types.
	$tmp = $f5{"weights", "p", $i};
	chop $tmp;
	@wp = split(/:/, $tmp);
	
	$tmp = $f5{"weights", "f", $i};
	chop $tmp;
	@wf = split(/:/, $tmp);
	
	$tmp = $f5{"weights", "if", $i};
	chop $tmp;
	@wif = split(/:/, $tmp);
	
				# Scan through each mcx unit
	for($j = 0; $j < $mcx{"num"}; ++$j)
	{
				# Retrieve weight

				# Get type of mcx unit
	    $type = $mcx{"type", $j};
				# Get index into array
	    $ind = $mcx{"type index", $j};

				# Get the weight out of the appropriate
				#  array
	    if($type eq "p")
	    {
		$val = $wp[$ind];
	    }
	    elsif($type eq "f")
	    {
		$val = $wf[$ind];
	    }
	    elsif($type eq "if")
	    {
		$val = $wif[$ind];
	    }
	    else
	    {
		die "dump_f5_weights(): bad type specified for unit $j ($type).\n";
	    };
				# Only write non-zero weights
	    if($val > 0)
	    {
		print FP "Mcx.$j $val\n";
	    };
	};
    };

    close FP;
};


############################################################

				# Check parameters
if(&interpret_parms() == 0 || $#ARGV == -1 || $ARGV[$#ARGV] =~ /-(\w)(.*)/)
{
    print "Usage: mknet [-]* \n";
    exit;
};

unlink("mcx/matlab.log");
				# Base file name
$base = $ARGV[$#ARGV];

				# Confirm there is a parameter file
$parms_fname = $base . ".parms.pl";
if(! -e $parms_fname)
{
    print "Error: no parameter file found.\n";
    exit;
};

require "$parms_fname";
print "Loaded parms file $parms_fname\n";


				# Open log file.
open(LOG_FP, ">$base.genmcx.log") ||
    die "Error opening log file ($base.genmcx.log).\n $!";



&init_data_structures();	# Initialize

&parse_mcx_input_files($base);	# Get specification files

				# Write the mcx matrices to disk
&generate_matlab_mcx_file($base);

&analyze_mcx();

				# Compute weights for each f5 unit
&loop_f5($base);

&dump_f5_weights($base);


close(LOG_FP);


[ return ]