text:
#include "m3/robots/humanoid.h"
#include "m3rt/base/component_factory.h"
#include "m3rt/base/toolbox.h"
#include "m3rt/base/m3rt_def.h"
//#include "m3rt/rt_system/rt_ros_service.h"
namespace m3
{

using namespace std;
using namespace KDL;

bool M3Humanoid::LinkDependentComponents()
{

	if (!M3Robot::LinkDependentComponents())
		return false;

	int nl=0;

	if (right_arm_name.size()!=0)
	{
		right_arm=(M3Arm*)factory->GetComponent(right_arm_name);
		if (right_arm!=NULL)
			nl++;
		else
			M3_WARN("M3Arm component %s declared for right_arm but could not be linked\n",
					right_arm_name.c_str());
		if (right_load_x6_component.size()!=0)
		{
			right_load_x6=(M3LoadX6*)factory->GetComponent(right_load_x6_component);
			if (right_load_x6!=NULL)
				nl++;
			else
				M3_WARN("M3LoadX6 component %s declared for right_arm but could not be linked\n",
						right_load_x6_component.c_str());
		}
	}
	if (left_arm_name.size()!=0)
	{
		left_arm=(M3Arm*)factory->GetComponent(left_arm_name);
		if (left_arm!=NULL)
			nl++;
		else
			M3_WARN("M3Arm component %s declared for left_arm but could not be linked\n",
					left_arm_name.c_str());

	}
	if (torso_name.size()!=0)
	{
		torso=(M3Torso*)factory->GetComponent(torso_name);
		if (torso!=NULL)
			nl++;
		else
			M3_WARN("M3Torso component %s declared for torso but could not be linked\n",
					torso_name.c_str());

	}
	if (head_name.size()!=0)
	{

		head=(M3Head*)factory->GetComponent(head_name);
		if (head!=NULL)
			nl++;
		else
			M3_WARN("M3Head component %s declared for head but could not be linked\n",
					head_name.c_str());

	}
	if (right_hand_name.size()!=0)
	{

		right_hand=(M3Hand*)factory->GetComponent(right_hand_name);
		if (right_hand!=NULL)
			nl++;
		else
			M3_WARN("M3RightHand component %s declared for head but could not be linked\n",
					right_hand_name.c_str());

	}
	if (left_hand_name.size()!=0)
	{

		left_hand=(M3Hand*)factory->GetComponent(left_hand_name);
		if (left_hand!=NULL)
			nl++;
		else
			M3_WARN("M3LeftHand component %s declared for head but could not be linked\n",
					left_hand_name.c_str());

	}
	if (nl>0)
		return true;

	M3_ERR("No M3Chains linked for M3Humanoid\n");

	return false;

}

void M3Humanoid::Startup()
{
	M3Robot::Startup();

	if(right_arm && right_arm->GetDynamatics())
	{
		for (int i=0; i<right_arm->GetNumDof(); i++)
		{
			status.mutable_right_arm()->add_g(0);

		}
		Vector com = right_arm->GetDynamatics()->GetPayloadCom();
		for (int i=0; i<3; i++)
		{
			status.mutable_right_arm()->add_end_pos(0);
			param.mutable_right_arm()->add_payload_com(com[i]);
		}

		param.mutable_right_arm()->set_payload_mass(right_arm->GetDynamatics()->GetPayloadMass());
		param.mutable_right_arm()->set_use_velocities(((M3DynamaticsParam*)right_arm->GetDynamatics()->GetParam())->use_velocities());
		param.mutable_right_arm()->set_use_accelerations(((M3DynamaticsParam*)right_arm->GetDynamatics()->GetParam())->use_accelerations());

		for (int i=0; i<9; i++)
		{
			status.mutable_right_arm()->add_end_rot(0);
		}

		for (int i=0; i<6; i++)
		{
			status.mutable_right_arm()->add_end_twist(0);
			param.mutable_right_arm()->add_payload_inertia(((M3DynamaticsParam*)right_arm->GetDynamatics()->GetParam())->payload_inertia(i));
		}

		for (int i=0; i<(6*right_arm->GetNumDof()); i++)
		{
			status.mutable_right_arm()->add_j(0);
		}
		J_right_arm = Jacobian(right_arm->GetNumDof());
		right_arm->GetDynamatics()->SetPriority(ARM_HEAD_DYNAMATICS_PRIORITY);
	}
	if(right_arm)
	{
		for (int i=0; i<right_arm->GetNumDof(); i++)
		{
			status.mutable_right_arm()->add_torque(0);
			status.mutable_right_arm()->add_torquedot(0);
			status.mutable_right_arm()->add_theta(0);
			status.mutable_right_arm()->add_thetadot(0);
			status.mutable_right_arm()->add_thetadotdot(0);
			status.mutable_right_arm()->add_pwm_cmd(0);
			command.mutable_right_arm()->add_cmd_enabled(true);
			command.mutable_right_arm()->add_q_desired(0);
			command.mutable_right_arm()->add_qdot_desired(0);
			command.mutable_right_arm()->add_tq_desired(0);
			command.mutable_right_arm()->add_q_stiffness(0);
			command.mutable_right_arm()->add_q_slew_rate(0);
			command.mutable_right_arm()->add_pwm_desired(0);
			command.mutable_right_arm()->add_ctrl_mode(JOINT_ARRAY_MODE_OFF);
			command.mutable_right_arm()->add_smoothing_mode(SMOOTHING_MODE_OFF);
		}
		torque_shm_right_arm.resize(right_arm->GetNumDof());
	}
	if(right_hand)
	{
		for (int i=0; i<right_hand->GetNumDof(); i++)
		{
			status.mutable_right_hand()->add_torque(0);
			status.mutable_right_hand()->add_torquedot(0);
			status.mutable_right_hand()->add_theta(0);
			status.mutable_right_hand()->add_thetadot(0);
			status.mutable_right_hand()->add_thetadotdot(0);
			status.mutable_right_hand()->add_pwm_cmd(0);
			command.mutable_right_hand()->add_cmd_enabled(true);
			command.mutable_right_hand()->add_q_desired(0);
			command.mutable_right_hand()->add_qdot_desired(0);
			command.mutable_right_hand()->add_tq_desired(0);
			command.mutable_right_hand()->add_q_stiffness(0);
			command.mutable_right_hand()->add_q_slew_rate(0);
			command.mutable_right_hand()->add_pwm_desired(0);
			command.mutable_right_hand()->add_ctrl_mode(JOINT_ARRAY_MODE_OFF);
			command.mutable_right_hand()->add_smoothing_mode(SMOOTHING_MODE_OFF);
		}
		torque_shm_right_hand.resize(right_hand->GetNumDof());
	}
	if(left_hand)
	{
		for (int i=0; i<left_hand->GetNumDof(); i++)
		{
			status.mutable_left_hand()->add_torque(0);
			status.mutable_left_hand()->add_torquedot(0);
			status.mutable_left_hand()->add_theta(0);
			status.mutable_left_hand()->add_thetadot(0);
			status.mutable_left_hand()->add_thetadotdot(0);
			status.mutable_left_hand()->add_pwm_cmd(0);
			command.mutable_left_hand()->add_cmd_enabled(true);
			command.mutable_left_hand()->add_q_desired(0);
			command.mutable_left_hand()->add_qdot_desired(0);
			command.mutable_left_hand()->add_tq_desired(0);
			command.mutable_left_hand()->add_q_stiffness(0);
			command.mutable_left_hand()->add_q_slew_rate(0);
			command.mutable_left_hand()->add_pwm_desired(0);
			command.mutable_left_hand()->add_ctrl_mode(JOINT_ARRAY_MODE_OFF);
			command.mutable_left_hand()->add_smoothing_mode(SMOOTHING_MODE_OFF);
		}
		torque_shm_left_hand.resize(left_hand->GetNumDof());
	}
	if(left_arm && left_arm->GetDynamatics())
	{
		for (int i=0; i<left_arm->GetNumDof(); i++)
		{
			status.mutable_left_arm()->add_g(0);

		}
		Vector com = left_arm->GetDynamatics()->GetPayloadCom();
		for (int i=0; i<3; i++)
		{
			status.mutable_left_arm()->add_end_pos(0);
			param.mutable_left_arm()->add_payload_com(com[i]);
		}

		param.mutable_left_arm()->set_payload_mass(left_arm->GetDynamatics()->GetPayloadMass());
		param.mutable_left_arm()->set_use_velocities(((M3DynamaticsParam*)left_arm->GetDynamatics()->GetParam())->use_velocities());
		param.mutable_left_arm()->set_use_accelerations(((M3DynamaticsParam*)left_arm->GetDynamatics()->GetParam())->use_accelerations());

		for (int i=0; i<9; i++)
		{
			status.mutable_left_arm()->add_end_rot(0);
		}

		for (int i=0; i<6; i++)
		{
			status.mutable_left_arm()->add_end_twist(0);
			param.mutable_left_arm()->add_payload_inertia(((M3DynamaticsParam*)left_arm->GetDynamatics()->GetParam())->payload_inertia(i));
		}

		for (int i=0; i<(6*left_arm->GetNumDof()); i++)
		{
			status.mutable_left_arm()->add_j(0);
		}
		J_left_arm = Jacobian(left_arm->GetNumDof());
		left_arm->GetDynamatics()->SetPriority(ARM_HEAD_DYNAMATICS_PRIORITY);
	}
	if(left_arm)
	{
		for (int i=0; i<left_arm->GetNumDof(); i++)
		{
			status.mutable_left_arm()->add_torque(0);
			status.mutable_left_arm()->add_torquedot(0);
			status.mutable_left_arm()->add_theta(0);
			status.mutable_left_arm()->add_thetadot(0);
			status.mutable_left_arm()->add_thetadotdot(0);
			status.mutable_left_arm()->add_pwm_cmd(0);
			command.mutable_left_arm()->add_cmd_enabled(true);
			command.mutable_left_arm()->add_q_desired(0);
			command.mutable_left_arm()->add_qdot_desired(0);
			command.mutable_left_arm()->add_tq_desired(0);
			command.mutable_left_arm()->add_q_stiffness(0);
			command.mutable_left_arm()->add_q_slew_rate(0);
			command.mutable_left_arm()->add_pwm_desired(0);
			command.mutable_left_arm()->add_ctrl_mode(JOINT_ARRAY_MODE_OFF);
			command.mutable_left_arm()->add_smoothing_mode(SMOOTHING_MODE_OFF);
		}
		torque_shm_left_arm.resize(left_arm->GetNumDof());
	}
	if(torso && torso->GetDynamatics())
	{
		for (int i=0; i<torso->GetNumDof(); i++)
		{
			status.mutable_torso()->add_g(0);
		}
		Vector com = torso->GetDynamatics()->GetPayloadCom();
		for (int i=0; i<3; i++)
		{
			status.mutable_torso()->add_end_pos(0);
			param.mutable_torso()->add_payload_com(com[i]);
		}

		param.mutable_torso()->set_payload_mass(torso->GetDynamatics()->GetPayloadMass());
		param.mutable_torso()->set_use_velocities(((M3DynamaticsParam*)torso->GetDynamatics()->GetParam())->use_velocities());
		param.mutable_torso()->set_use_accelerations(((M3DynamaticsParam*)torso->GetDynamatics()->GetParam())->use_accelerations());

		for (int i=0; i<9; i++)
		{
			status.mutable_torso()->add_end_rot(0);
		}

		for (int i=0; i<6; i++)
		{
			status.mutable_torso()->add_end_twist(0);
			param.mutable_torso()->add_payload_inertia(((M3DynamaticsParam*)torso->GetDynamatics()->GetParam())->payload_inertia(i));
		}

		for (int i=0; i<(6*torso->GetNumDof()); i++)
		{
			status.mutable_torso()->add_j(0);
		}
		J_torso = Jacobian(torso->GetNumDof());
	}
	if(torso)
	{
		for (int i=0; i<torso->GetNumDof(); i++)
		{
			status.mutable_torso()->add_torque(0);
			status.mutable_torso()->add_torquedot(0);
			status.mutable_torso()->add_theta(0);
			status.mutable_torso()->add_thetadot(0);
			status.mutable_torso()->add_thetadotdot(0);
			status.mutable_torso()->add_pwm_cmd(0);
			command.mutable_torso()->add_cmd_enabled(true);
			command.mutable_torso()->add_q_desired(0);
			command.mutable_torso()->add_qdot_desired(0);
			command.mutable_torso()->add_tq_desired(0);
			command.mutable_torso()->add_q_stiffness(0);
			command.mutable_torso()->add_q_slew_rate(0);
			command.mutable_torso()->add_pwm_desired(0);
			command.mutable_torso()->add_ctrl_mode(JOINT_ARRAY_MODE_OFF);
			command.mutable_torso()->add_smoothing_mode(SMOOTHING_MODE_OFF);
		}
		torque_shm_torso.resize(torso->GetNumDof());
	}

	if(head && head->GetDynamatics())
	{
		for (int i=0; i<head->GetDynamatics()->GetNumDof(); i++)//head->GetNumDof()
		{
			status.mutable_head()->add_g(0);
		}
		Vector com = head->GetDynamatics()->GetPayloadCom();
		for (int i=0; i<3; i++)
		{
			status.mutable_head()->add_end_pos(0);
			param.mutable_head()->add_payload_com(com[i]);
			status.add_left_eye_pos(0);
			status.add_right_eye_pos(0);
		}

		param.mutable_head()->set_payload_mass(head->GetDynamatics()->GetPayloadMass());
		param.mutable_head()->set_use_velocities(((M3DynamaticsParam*)head->GetDynamatics()->GetParam())->use_velocities());
		param.mutable_head()->set_use_accelerations(((M3DynamaticsParam*)head->GetDynamatics()->GetParam())->use_accelerations());

		for (int i=0; i<9; i++)
		{
			status.mutable_head()->add_end_rot(0);
			status.add_left_eye_rot(0);
			status.add_right_eye_rot(0);
		}

		for (int i=0; i<6; i++)
		{
			status.mutable_head()->add_end_twist(0);
			param.mutable_head()->add_payload_inertia(((M3DynamaticsParam*)head->GetDynamatics()->GetParam())->payload_inertia(i));
		}

		for (int i=0; i<(6*head->GetDynamatics()->GetNumDof()); i++)//head->GetNumDof()
		{
			status.mutable_head()->add_j(0);
		}
		J_head = Jacobian(head->GetNumDof());
		head->GetDynamatics()->SetPriority(ARM_HEAD_DYNAMATICS_PRIORITY);
	}
	if(head)
	{
		for (int i=0; i<head->GetNumDof(); i++)
		{
			status.mutable_head()->add_torque(0);
			status.mutable_head()->add_torquedot(0);
			status.mutable_head()->add_theta(0);
			status.mutable_head()->add_thetadot(0);
			status.mutable_head()->add_thetadotdot(0);
			status.mutable_head()->add_pwm_cmd(0);
			command.mutable_head()->add_cmd_enabled(true);
			command.mutable_head()->add_q_desired(0);
			command.mutable_head()->add_qdot_desired(0);
			command.mutable_head()->add_tq_desired(0);
			command.mutable_head()->add_q_stiffness(0);
			command.mutable_head()->add_q_slew_rate(0);
			command.mutable_head()->add_pwm_desired(0);
			command.mutable_head()->add_ctrl_mode(JOINT_ARRAY_MODE_OFF);
			command.mutable_head()->add_smoothing_mode(SMOOTHING_MODE_OFF);
		}
		left_eye_offset = head->GetLeftEyeOffset();
		right_eye_offset = head->GetRightEyeOffset();
		angle_shm_head.resize(head->GetNumDof());
		slew_rate_shm_head.resize(head->GetNumDof());
	}

	if(startup_motor_pwr_on)
	  SetMotorPowerOn();
	SetStateSafeOp();

}

void M3Humanoid::Shutdown()
{

}

void M3Humanoid::StepCommand()
{
	if (IsStateSafeOp() || IsStateError())
		return;

	pwr->SetMotorEnable(command.enable_motor());
	if(right_hand)
	{
		for (int i=0; i<right_hand->GetNumDof(); i++)
		{
		    // A.H: Detect if the command if NULL (no changed), then do not apply. This is useful if multiple client
		    // publishes command to the bot interface for example, and one is publishing to the head only, the other to the arms.
		    // So I just check if the fields cmd_enabled is false (FYI it is initialized at true for backward compatibility)
		    if(command.right_hand().ctrl_mode(i)== JOINT_ARRAY_MODE_OFF && command.right_hand().cmd_enabled(i) == false) continue;
		    if (command.right_hand().ctrl_mode(i) == JOINT_ARRAY_MODE_TORQUE_SHM || force_shm_r_hand)
		    {
			if (enable_shm_r_hand)
			{
			  ((M3JointArrayCommand*)right_hand->GetCommand())->set_tq_desired(i, torque_shm_right_hand(i));
			  ((M3JointArrayCommand*)right_hand->GetCommand())->set_ctrl_mode(i, JOINT_ARRAY_MODE_TORQUE);
			} else {
			  ((M3JointArrayCommand*)right_hand->GetCommand())->set_tq_desired(i, 0);
			  ((M3JointArrayCommand*)right_hand->GetCommand())->set_ctrl_mode(i, JOINT_ARRAY_MODE_OFF);
			}
		    } else {
			((M3JointArrayCommand*)right_hand->GetCommand())->set_tq_desired(i, command.right_hand().tq_desired(i));
			((M3JointArrayCommand*)right_hand->GetCommand())->set_ctrl_mode(i, command.right_hand().ctrl_mode(i));

		    }
		    ((M3JointArrayCommand*)right_hand->GetCommand())->set_smoothing_mode(i, command.right_hand().smoothing_mode(i));
		    ((M3JointArrayCommand*)right_hand->GetCommand())->set_q_stiffness(i, command.right_hand().q_stiffness(i));
		    ((M3JointArrayCommand*)right_hand->GetCommand())->set_q_desired(i, command.right_hand().q_desired(i));
		    ((M3JointArrayCommand*)right_hand->GetCommand())->set_qdot_desired(i, command.right_hand().qdot_desired(i));
		    ((M3JointArrayCommand*)right_hand->GetCommand())->set_q_slew_rate(i, command.right_hand().q_slew_rate(i));
		    ((M3JointArrayCommand*)right_hand->GetCommand())->set_pwm_desired(i, command.right_hand().pwm_desired(i));
		}
		for (int i=0;i<command.right_hand().vias_size();i++)
		{
			M3JointVia * via;
			via = ((M3JointArrayCommand*)right_hand->GetCommand())->add_vias();
			*via = command.right_hand().vias(i);
		}
		command.mutable_right_hand()->clear_vias();
	}
	if(left_hand)
	{
		for (int i=0; i<left_hand->GetNumDof(); i++)
		{
		    if(command.left_hand().ctrl_mode(i)== JOINT_ARRAY_MODE_OFF && command.left_hand().cmd_enabled(i) == false) continue;

		    if (command.left_hand().ctrl_mode(i) == JOINT_ARRAY_MODE_TORQUE_SHM || force_shm_l_hand)
		    {
			if (enable_shm_l_hand)
			{
			  ((M3JointArrayCommand*)left_hand->GetCommand())->set_tq_desired(i, torque_shm_left_hand(i));
			  ((M3JointArrayCommand*)left_hand->GetCommand())->set_ctrl_mode(i, JOINT_ARRAY_MODE_TORQUE);
			} else {
			  ((M3JointArrayCommand*)left_hand->GetCommand())->set_tq_desired(i, 0);
			  ((M3JointArrayCommand*)left_hand->GetCommand())->set_ctrl_mode(i, JOINT_ARRAY_MODE_OFF);
			}
		    } else {
			((M3JointArrayCommand*)left_hand->GetCommand())->set_tq_desired(i, command.left_hand().tq_desired(i));
			((M3JointArrayCommand*)left_hand->GetCommand())->set_ctrl_mode(i, command.left_hand().ctrl_mode(i));

		    }
		    ((M3JointArrayCommand*)left_hand->GetCommand())->set_smoothing_mode(i, command.left_hand().smoothing_mode(i));
		    ((M3JointArrayCommand*)left_hand->GetCommand())->set_q_stiffness(i, command.left_hand().q_stiffness(i));
		    ((M3JointArrayCommand*)left_hand->GetCommand())->set_q_desired(i, command.left_hand().q_desired(i));
		    ((M3JointArrayCommand*)left_hand->GetCommand())->set_qdot_desired(i, command.left_hand().qdot_desired(i));
		    ((M3JointArrayCommand*)left_hand->GetCommand())->set_q_slew_rate(i, command.left_hand().q_slew_rate(i));
		    ((M3JointArrayCommand*)left_hand->GetCommand())->set_pwm_desired(i, command.left_hand().pwm_desired(i));
		}
		for (int i=0;i<command.left_hand().vias_size();i++)
		{
			M3JointVia * via;
			via = ((M3JointArrayCommand*)left_hand->GetCommand())->add_vias();
			*via = command.left_hand().vias(i);
		}
		command.mutable_left_hand()->clear_vias();
	}
	if(torso)
	{
		for (int i=0; i<torso->GetNumDof(); i++)
		{
		    if(command.torso().ctrl_mode(i)== JOINT_ARRAY_MODE_OFF && command.torso().cmd_enabled(i) == false) continue;
		     if (command.torso().ctrl_mode(i) == JOINT_ARRAY_MODE_TORQUE_SHM || force_shm_torso)
		     {
		       if (enable_shm_torso)
		       {
			((M3JointArrayCommand*)torso->GetCommand())->set_tq_desired(i, torque_shm_torso(i));
			((M3JointArrayCommand*)torso->GetCommand())->set_ctrl_mode(i, JOINT_ARRAY_MODE_TORQUE);
		       } else {
			 ((M3JointArrayCommand*)torso->GetCommand())->set_tq_desired(i, 0);
			 ((M3JointArrayCommand*)torso->GetCommand())->set_ctrl_mode(i, JOINT_ARRAY_MODE_OFF);
		       }
		     } else {
			((M3JointArrayCommand*)torso->GetCommand())->set_tq_desired(i, command.torso().tq_desired(i));
			((M3JointArrayCommand*)torso->GetCommand())->set_ctrl_mode(i, command.torso().ctrl_mode(i));
		     }
		     ((M3JointArrayCommand*)torso->GetCommand())->set_smoothing_mode(i, command.torso().smoothing_mode(i));
		    ((M3JointArrayCommand*)torso->GetCommand())->set_q_stiffness(i, command.torso().q_stiffness(i));
		    ((M3JointArrayCommand*)torso->GetCommand())->set_q_desired(i, command.torso().q_desired(i));
		    ((M3JointArrayCommand*)torso->GetCommand())->set_qdot_desired(i, command.torso().qdot_desired(i));
		    ((M3JointArrayCommand*)torso->GetCommand())->set_q_slew_rate(i, command.torso().q_slew_rate(i));
		    ((M3JointArrayCommand*)torso->GetCommand())->set_pwm_desired(i, command.torso().pwm_desired(i));
		}
		for (int i=0;i<command.torso().vias_size();i++)
		{
			M3JointVia * via;
			via = ((M3JointArrayCommand*)torso->GetCommand())->add_vias();
			*via = command.torso().vias(i);
		}
		command.mutable_torso()->clear_vias();
	}

	if(head)
	{
		for (int i=0; i<head->GetNumDof(); i++)
		{
		  if(command.head().ctrl_mode(i)== JOINT_ARRAY_MODE_OFF && command.head().cmd_enabled(i) == false) continue;
		    if (command.head().ctrl_mode(i) == JOINT_ARRAY_MODE_ANGLE_SHM || force_shm_head)
		    {
		       if (enable_shm_head)
		       {
			((M3JointArrayCommand*)head->GetCommand())->set_q_desired(i, angle_shm_head(i));
			((M3JointArrayCommand*)head->GetCommand())->set_ctrl_mode(i, JOINT_ARRAY_MODE_THETA);
			((M3JointArrayCommand*)head->GetCommand())->set_q_slew_rate(i, slew_rate_shm_head(i));
		       } else {
			 //((M3JointArrayCommand*)head->GetCommand())->set_q_desired(i, 0);
			 //((M3JointArrayCommand*)head->GetCommand())->set_q_slew_rate(i, command.head().q_slew_rate(i));
			 ((M3JointArrayCommand*)head->GetCommand())->set_ctrl_mode(i, JOINT_ARRAY_MODE_OFF);
		       }
		     } else {
			((M3JointArrayCommand*)head->GetCommand())->set_q_desired(i, command.head().q_desired(i));
			((M3JointArrayCommand*)head->GetCommand())->set_q_slew_rate(i, command.head().q_slew_rate(i));
			((M3JointArrayCommand*)head->GetCommand())->set_ctrl_mode(i, command.head().ctrl_mode(i));
		     }
		    ((M3JointArrayCommand*)head->GetCommand())->set_smoothing_mode(i, command.head().smoothing_mode(i));
		    ((M3JointArrayCommand*)head->GetCommand())->set_tq_desired(i, command.head().tq_desired(i));
		    ((M3JointArrayCommand*)head->GetCommand())->set_q_stiffness(i, command.head().q_stiffness(i));
		    ((M3JointArrayCommand*)head->GetCommand())->set_qdot_desired(i, command.head().qdot_desired(i));
		    ((M3JointArrayCommand*)head->GetCommand())->set_pwm_desired(i, command.head().pwm_desired(i));
		}
		for (int i=0;i<command.head().vias_size();i++)
		{
			M3JointVia * via;
			via = ((M3JointArrayCommand*)head->GetCommand())->add_vias();
			*via = command.head().vias(i);
		}
		command.mutable_head()->clear_vias();
	}

	if(right_arm)
	{
		for (int i=0; i<right_arm->GetNumDof(); i++)
		{
		  if(command.right_arm().ctrl_mode(i)== JOINT_ARRAY_MODE_OFF && command.right_arm().cmd_enabled(i) == false) continue;
		    if (command.right_arm().ctrl_mode(i) == JOINT_ARRAY_MODE_TORQUE_SHM || force_shm_r_arm)
		    {
			if (enable_shm_r_arm)
			{
			  ((M3JointArrayCommand*)right_arm->GetCommand())->set_tq_desired(i, torque_shm_right_arm(i));
			  ((M3JointArrayCommand*)right_arm->GetCommand())->set_ctrl_mode(i, JOINT_ARRAY_MODE_TORQUE);
			} else {
			  ((M3JointArrayCommand*)right_arm->GetCommand())->set_tq_desired(i, 0);
			  ((M3JointArrayCommand*)right_arm->GetCommand())->set_ctrl_mode(i, JOINT_ARRAY_MODE_OFF);
			}
		    } else {
			((M3JointArrayCommand*)right_arm->GetCommand())->set_tq_desired(i, command.right_arm().tq_desired(i));
			((M3JointArrayCommand*)right_arm->GetCommand())->set_ctrl_mode(i, command.right_arm().ctrl_mode(i));

		    }
		    ((M3JointArrayCommand*)right_arm->GetCommand())->set_smoothing_mode(i, command.right_arm().smoothing_mode(i));
		    ((M3JointArrayCommand*)right_arm->GetCommand())->set_q_stiffness(i, command.right_arm().q_stiffness(i));
		    ((M3JointArrayCommand*)right_arm->GetCommand())->set_q_desired(i, command.right_arm().q_desired(i));
		    ((M3JointArrayCommand*)right_arm->GetCommand())->set_qdot_desired(i, command.right_arm().qdot_desired(i));
		    ((M3JointArrayCommand*)right_arm->GetCommand())->set_q_slew_rate(i, command.right_arm().q_slew_rate(i));
		    ((M3JointArrayCommand*)right_arm->GetCommand())->set_pwm_desired(i, command.right_arm().pwm_desired(i));
		}
		for (int i=0;i<command.right_arm().vias_size();i++)
		{
			M3JointVia * via;
			via = ((M3JointArrayCommand*)right_arm->GetCommand())->add_vias();
			*via = command.right_arm().vias(i);
		}
		command.mutable_right_arm()->clear_vias();
	}

	if(left_arm)
	{
		for (int i=0; i<left_arm->GetNumDof(); i++)
		{
		  if(command.left_arm().ctrl_mode(i)== JOINT_ARRAY_MODE_OFF && command.left_arm().cmd_enabled(i) == false) continue;
		    if (command.left_arm().ctrl_mode(i) == JOINT_ARRAY_MODE_TORQUE_SHM || force_shm_l_arm)
		    {
		      if (enable_shm_l_arm)
			{
			  ((M3JointArrayCommand*)left_arm->GetCommand())->set_tq_desired(i, torque_shm_left_arm(i));
			  ((M3JointArrayCommand*)left_arm->GetCommand())->set_ctrl_mode(i, JOINT_ARRAY_MODE_TORQUE);
			} else {
			  ((M3JointArrayCommand*)left_arm->GetCommand())->set_tq_desired(i, 0);
			  ((M3JointArrayCommand*)left_arm->GetCommand())->set_ctrl_mode(i, JOINT_ARRAY_MODE_OFF);
			}
		    } else {
			((M3JointArrayCommand*)left_arm->GetCommand())->set_tq_desired(i, command.left_arm().tq_desired(i));
			((M3JointArrayCommand*)left_arm->GetCommand())->set_ctrl_mode(i, command.left_arm().ctrl_mode(i));
		    }
		    ((M3JointArrayCommand*)left_arm->GetCommand())->set_smoothing_mode(i, command.left_arm().smoothing_mode(i));
		    ((M3JointArrayCommand*)left_arm->GetCommand())->set_q_stiffness(i, command.left_arm().q_stiffness(i));
		    ((M3JointArrayCommand*)left_arm->GetCommand())->set_q_desired(i, command.left_arm().q_desired(i));
		    ((M3JointArrayCommand*)left_arm->GetCommand())->set_qdot_desired(i, command.left_arm().qdot_desired(i));
		    ((M3JointArrayCommand*)left_arm->GetCommand())->set_q_slew_rate(i, command.left_arm().q_slew_rate(i));
		    ((M3JointArrayCommand*)left_arm->GetCommand())->set_pwm_desired(i, command.left_arm().pwm_desired(i));
		}
		for (int i=0;i<command.left_arm().vias_size();i++)
		{
			M3JointVia * via;
			via = ((M3JointArrayCommand*)left_arm->GetCommand())->add_vias();
			*via = command.left_arm().vias(i);
		}
		command.mutable_left_arm()->clear_vias();
	}

}

void M3Humanoid::StepStatus()
{
	if (IsStateError())
		return;

	grav_end_torso[0] = 0;
	grav_end_torso[1] = 0;
	grav_end_torso[2] = 0;

////////////////////////////////////////////////////////
////  1> Copy status from chains and dynamatics to humanoid status
////  2> Transform End Frame, Twist, and Jacobian values for each chain
////       to the world reference frame
////  3> Set gravity value for chains depending on each chain's base link orientation in world frame
////  4> Apply correct Wrenches at Torso's End Frame to compensate for arm/head torques
///////////////////////////////////////////////////////
////  chain => parent offsets are defined in humanoid yaml file using base_rotation_in_parent
////			and base_translation_in_parent
////  For a humanoid with torso, arms, and head offsets are defined as follows:
////  torso_offset = Transform from torso_T_0 => world_origin
////  right_arm_offset = Transform from right_arm_T_0 => torso_T_4
////  left_arm_offset = Transform from left_arm_T_0 => torso_T_4
////  head_offset = Transform from head_T_0 => torso_T_4
///////////////////////////////////////////////////////
////  For a humanoid without torso:
////  right_arm_offset = Transform from right_arm_T_0 => world_origin
////  left_arm_offset = Transform from left_arm_T_0 => world_origin
////  head_offset = Transform from head_T_0 => world_origin
///////////////////////////////////////////////////////

	if(torso)
	{
		for (int i=0; i<torso->GetNumDof(); i++)
		{
			status.mutable_torso()->set_torque(i,((M3JointArrayStatus*)torso->GetStatus())->torque(i));
			status.mutable_torso()->set_torquedot(i,((M3JointArrayStatus*)torso->GetStatus())->torquedot(i));
			status.mutable_torso()->set_theta(i,((M3JointArrayStatus*)torso->GetStatus())->theta(i));
			status.mutable_torso()->set_thetadot(i,((M3JointArrayStatus*)torso->GetStatus())->thetadot(i));
			status.mutable_torso()->set_thetadotdot(i,((M3JointArrayStatus*)torso->GetStatus())->thetadotdot(i));
			status.mutable_torso()->set_pwm_cmd(i,((M3JointArrayStatus*)torso->GetStatus())->pwm_cmd(i));
		}
		status.mutable_torso()->set_completed_spline_idx(((M3JointArrayStatus*)torso->GetStatus())->completed_spline_idx());

		if(torso->GetDynamatics())
		{
			for (int i=0; i<3; i++)
				((M3DynamaticsParam*)torso->GetDynamatics()->GetParam())->set_payload_com(i,
					 param.torso().payload_com(i));
			for (int i=0; i<6; i++)
				((M3DynamaticsParam*)torso->GetDynamatics()->GetParam())->set_payload_inertia(i,
					 param.torso().payload_inertia(i));
			((M3DynamaticsParam*)torso->GetDynamatics()->GetParam())->set_payload_mass(param.torso().payload_mass());
		}
	}

	if(torso && torso->GetDynamatics())
	{
		for (int i=0; i<6; i++)
		{
			for (int j=0; j<torso->GetDynamatics()->GetNumDof(); j++)
				J_torso(i,j) =
				 ((M3DynamaticsStatus*)torso->GetDynamatics()->GetStatus())->j((i*torso->GetDynamatics()->GetNumDof())+(j));
		}

		for (int i=0; i<torso->GetDynamatics()->GetNumDof(); i++)
			status.mutable_torso()->set_g(i,((M3DynamaticsStatus*)torso->GetDynamatics()->GetStatus())->g(i));

		// torso_end_frame will now be Torso_T4 => World_Origin
		torso_end_frame = torso_offset * Frame(torso->GetDynamatics()->GetEndRot(),torso->GetDynamatics()->GetEndPos());
		// grav_end_torso now gravity vector in Torso_T4 frame
		grav_end_torso = torso_end_frame.M.Inverse() * grav_world;
		torso_twist = torso_offset*torso->GetDynamatics()->GetEndTwist();
		J_torso.changeRefFrame(torso_offset);
	}

	if (right_arm)
	{
		for (int i=0; i<right_arm->GetNumDof(); i++)
		{
			status.mutable_right_arm()->set_torque(i,((M3JointArrayStatus*)right_arm->GetStatus())->torque(i));
			status.mutable_right_arm()->set_torquedot(i,((M3JointArrayStatus*)right_arm->GetStatus())->torquedot(i));
			status.mutable_right_arm()->set_theta(i,((M3JointArrayStatus*)right_arm->GetStatus())->theta(i));
			status.mutable_right_arm()->set_thetadot(i,((M3JointArrayStatus*)right_arm->GetStatus())->thetadot(i));
			status.mutable_right_arm()->set_thetadotdot(i,((M3JointArrayStatus*)right_arm->GetStatus())->thetadotdot(i));
			status.mutable_right_arm()->set_pwm_cmd(i,((M3JointArrayStatus*)right_arm->GetStatus())->pwm_cmd(i));
		}
		status.mutable_right_arm()->set_completed_spline_idx(((M3JointArrayStatus*)right_arm->GetStatus())->completed_spline_idx());

		if(right_arm->GetDynamatics())
		{
			if (right_use_loadx6_instead_of_payload)
			{
			    if (right_load_x6)
			    {
			      Wrench * load_wrench = right_load_x6->GetWrench();
			      Wrench end_wrench;
			      // TODO: make this a transform.. hard coding for now
			      //end_wrench.force(0) = -load_wrench->force(1)/1000.0;
			      //end_wrench.torque(0) = -load_wrench->torque(1)/1000.0;
			      //end_wrench.force(1) = -load_wrench->force(0)/1000.0;
			      //end_wrench.torque(1) = -load_wrench->torque(0)/1000.0;
			      //end_wrench.force(2) = load_wrench->force(2)/1000.0;
			      //end_wrench.torque(2) = load_wrench->torque(2)/1000.0;
			      end_wrench.force(0) = 0;
			      end_wrench.torque(0) = 0;
			      end_wrench.force(1) = 0;
			      end_wrench.torque(1) = 0;
			      end_wrench.force(2) = 0;
			      end_wrench.torque(2) = 0;
			      right_arm->GetDynamatics()->SetEndWrench(end_wrench);
			    }
			    for (int i=0; i<3; i++)
				    ((M3DynamaticsParam*)right_arm->GetDynamatics()->GetParam())->set_payload_com(i,0);
			    for (int i=0; i<6; i++)
				    ((M3DynamaticsParam*)right_arm->GetDynamatics()->GetParam())->set_payload_inertia(i,0);
			      ((M3DynamaticsParam*)right_arm->GetDynamatics()->GetParam())->set_payload_mass(0);
			}
			else
			{
			    for (int i=0; i<3; i++)
				    ((M3DynamaticsParam*)right_arm->GetDynamatics()->GetParam())->set_payload_com(i,
					    param.right_arm().payload_com(i));
			    for (int i=0; i<6; i++)
				    ((M3DynamaticsParam*)right_arm->GetDynamatics()->GetParam())->set_payload_inertia(i,
					    param.right_arm().payload_inertia(i));
			      ((M3DynamaticsParam*)right_arm->GetDynamatics()->GetParam())->set_payload_mass(param.right_arm().payload_mass());
			}
		}
	}

	if(right_arm && right_arm->GetDynamatics())
	{
		for (int i=0; i<right_arm->GetDynamatics()->GetNumDof(); i++)
			status.mutable_right_arm()->set_g(i,((M3DynamaticsStatus*)right_arm->GetDynamatics()->GetStatus())->g(i));

		for (int i=0; i<6; i++)
		{
			for (int j=0; j<right_arm->GetDynamatics()->GetNumDof(); j++)
				J_right_arm(i,j) =
				 ((M3DynamaticsStatus*)right_arm->GetDynamatics()->GetStatus())->j((i*right_arm->GetDynamatics()->GetNumDof())+(j));
		}
		// right_arm_end_frame will now be right_arm_T8 => right_arm_base
		right_arm_end_frame = right_arm_offset *
				 Frame(right_arm->GetDynamatics()->GetEndRot(),right_arm->GetDynamatics()->GetEndPos());
		right_arm_twist = right_arm_offset * right_arm->GetDynamatics()->GetEndTwist();
		J_right_arm.changeRefFrame(right_arm_offset);
		if(torso && torso->GetDynamatics())
		{
			// right_arm_end_frame will now be right_arm_T8 => world_origin
			right_arm_end_frame = torso_end_frame * right_arm_end_frame;
			// setting grav vector in right_arm_T0
			right_arm->GetDynamatics()->SetGrav(right_arm_offset.M.Inverse() * grav_end_torso);
			right_arm_twist = torso_end_frame * right_arm_twist;
			J_right_arm.changeRefFrame(torso_end_frame);
		} else
			right_arm->GetDynamatics()->SetGrav(right_arm_offset.M.Inverse() * grav_world);
	}

	if(left_arm)
	{
		for (int i=0; i<left_arm->GetNumDof(); i++)
		{
			status.mutable_left_arm()->set_torque(i,((M3JointArrayStatus*)left_arm->GetStatus())->torque(i));
			status.mutable_left_arm()->set_torquedot(i,((M3JointArrayStatus*)left_arm->GetStatus())->torquedot(i));
			status.mutable_left_arm()->set_theta(i,((M3JointArrayStatus*)left_arm->GetStatus())->theta(i));
			status.mutable_left_arm()->set_thetadot(i,((M3JointArrayStatus*)left_arm->GetStatus())->thetadot(i));
			status.mutable_left_arm()->set_thetadotdot(i,((M3JointArrayStatus*)left_arm->GetStatus())->thetadotdot(i));
			status.mutable_left_arm()->set_pwm_cmd(i,((M3JointArrayStatus*)left_arm->GetStatus())->pwm_cmd(i));
		}
		status.mutable_left_arm()->set_completed_spline_idx(((M3JointArrayStatus*)left_arm->GetStatus())->completed_spline_idx());

		if(left_arm->GetDynamatics())
		{
			for (int i=0; i<3; i++)
				((M3DynamaticsParam*)left_arm->GetDynamatics()->GetParam())->set_payload_com(i,
					 param.left_arm().payload_com(i));
			for (int i=0; i<6; i++)
				((M3DynamaticsParam*)left_arm->GetDynamatics()->GetParam())->set_payload_inertia(i,
					 param.left_arm().payload_inertia(i));
			((M3DynamaticsParam*)left_arm->GetDynamatics()->GetParam())->set_payload_mass(param.left_arm().payload_mass());
		}
	}

	if(left_arm && left_arm->GetDynamatics())
	{
		for (int i=0; i<6; i++)
		{
			for (int j=0; j<left_arm->GetDynamatics()->GetNumDof(); j++)
				J_left_arm(i,j) =
				 ((M3DynamaticsStatus*)left_arm->GetDynamatics()->GetStatus())->j((i*left_arm->GetDynamatics()->GetNumDof())+(j));
		}

		for (int i=0; i<left_arm->GetDynamatics()->GetNumDof(); i++)
		{
			status.mutable_left_arm()->set_g(i,((M3DynamaticsStatus*)left_arm->GetDynamatics()->GetStatus())->g(i));
		}
		// left_arm_end_frame will now be left_arm_T8 => left_arm_base
		left_arm_end_frame = left_arm_offset *
				 Frame(left_arm->GetDynamatics()->GetEndRot(),left_arm->GetDynamatics()->GetEndPos());
		left_arm_twist = left_arm_offset * left_arm->GetDynamatics()->GetEndTwist();
		J_left_arm.changeRefFrame(left_arm_offset);
		if(torso && torso->GetDynamatics())
		{
			// left_arm_end_frame will now be left_arm_T8 => world_origin
			left_arm_end_frame = torso_end_frame * left_arm_end_frame;
			// setting grav vector in left_arm_T0
			left_arm->GetDynamatics()->SetGrav(left_arm_offset.M.Inverse() * grav_end_torso);
			left_arm_twist = torso_end_frame * left_arm_twist;
			J_left_arm.changeRefFrame(torso_end_frame);
		} else left_arm->GetDynamatics()->SetGrav(left_arm_offset.M.Inverse() * grav_world);
	}

	if(head)
	{
		for (int i=0; i<head->GetNumDof(); i++)
		{
			status.mutable_head()->set_torque(i,((M3JointArrayStatus*)head->GetStatus())->torque(i));
			status.mutable_head()->set_torquedot(i,((M3JointArrayStatus*)head->GetStatus())->torquedot(i));
			status.mutable_head()->set_theta(i,((M3JointArrayStatus*)head->GetStatus())->theta(i));
			status.mutable_head()->set_thetadot(i,((M3JointArrayStatus*)head->GetStatus())->thetadot(i));
			status.mutable_head()->set_thetadotdot(i,((M3JointArrayStatus*)head->GetStatus())->thetadotdot(i));
			status.mutable_head()->set_pwm_cmd(i,((M3JointArrayStatus*)head->GetStatus())->pwm_cmd(i));
		}
		status.mutable_head()->set_completed_spline_idx(((M3JointArrayStatus*)head->GetStatus())->completed_spline_idx());

		if(head->GetDynamatics())
		{
			for (int i=0; i<3; i++)
				((M3DynamaticsParam*)head->GetDynamatics()->GetParam())->set_payload_com(i,
					 param.head().payload_com(i));
			for (int i=0; i<6; i++)
				((M3DynamaticsParam*)head->GetDynamatics()->GetParam())->set_payload_inertia(i,
					 param.head().payload_inertia(i));
			((M3DynamaticsParam*)head->GetDynamatics()->GetParam())->set_payload_mass(param.head().payload_mass());
		}
	}
	if(right_hand)
	{
		for (int i=0; i<right_hand->GetNumDof(); i++)
		{
			status.mutable_right_hand()->set_torque(i,((M3JointArrayStatus*)right_hand->GetStatus())->torque(i));
			status.mutable_right_hand()->set_torquedot(i,((M3JointArrayStatus*)right_hand->GetStatus())->torquedot(i));
			status.mutable_right_hand()->set_theta(i,((M3JointArrayStatus*)right_hand->GetStatus())->theta(i));
			status.mutable_right_hand()->set_thetadot(i,((M3JointArrayStatus*)right_hand->GetStatus())->thetadot(i));
			status.mutable_right_hand()->set_thetadotdot(i,((M3JointArrayStatus*)right_hand->GetStatus())->thetadotdot(i));
			status.mutable_right_hand()->set_pwm_cmd(i,((M3JointArrayStatus*)right_hand->GetStatus())->pwm_cmd(i));
		}
		status.mutable_right_hand()->set_completed_spline_idx(((M3JointArrayStatus*)right_hand->GetStatus())->completed_spline_idx());
	}
	if(left_hand)
	{
		for (int i=0; i<left_hand->GetNumDof(); i++)
		{
			status.mutable_left_hand()->set_torque(i,((M3JointArrayStatus*)left_hand->GetStatus())->torque(i));
			status.mutable_left_hand()->set_torquedot(i,((M3JointArrayStatus*)left_hand->GetStatus())->torquedot(i));
			status.mutable_left_hand()->set_theta(i,((M3JointArrayStatus*)left_hand->GetStatus())->theta(i));
			status.mutable_left_hand()->set_thetadot(i,((M3JointArrayStatus*)left_hand->GetStatus())->thetadot(i));
			status.mutable_left_hand()->set_thetadotdot(i,((M3JointArrayStatus*)left_hand->GetStatus())->thetadotdot(i));
			status.mutable_left_hand()->set_pwm_cmd(i,((M3JointArrayStatus*)left_hand->GetStatus())->pwm_cmd(i));
		}
		status.mutable_left_hand()->set_completed_spline_idx(((M3JointArrayStatus*)left_hand->GetStatus())->completed_spline_idx());
	}
	if(head && head->GetDynamatics())
	{
		for (int i=0; i<head->GetDynamatics()->GetNumDof(); i++)
			status.mutable_head()->set_g(i,((M3DynamaticsStatus*)head->GetDynamatics()->GetStatus())->g(i));

		for (int i=0; i<6; i++)
		{
			for (int j=0; j<head->GetDynamatics()->GetNumDof(); j++)
				J_head(i,j) =
				 ((M3DynamaticsStatus*)head->GetDynamatics()->GetStatus())->j((i*head->GetDynamatics()->GetNumDof())+(j));
		}
		// head_end_frame will now be head_T5 => head_base
		head_end_frame = head_offset *
				 Frame(head->GetDynamatics()->GetEndRot(),head->GetDynamatics()->GetEndPos());
		head_twist = head_offset * head->GetDynamatics()->GetEndTwist();
		J_head.changeRefFrame(head_offset);

		if(torso && torso->GetDynamatics())
		{
			// head_end_frame will now be head_T5 => world_origin
			head_base_2_world_frame=torso_end_frame*head_offset;
			head_end_frame = torso_end_frame * head_end_frame;
			// setting grav vector in head_T0
			head->GetDynamatics()->SetGrav(head_offset.M.Inverse() * grav_end_torso);
			head_twist = torso_end_frame * head_twist;
			J_head.changeRefFrame(torso_end_frame);

		} else head->GetDynamatics()->SetGrav(head_offset.M.Inverse() * grav_world);

////////////////////////////
/////// Eyes (*)--(*) //////
////////////////////////////

		Frame T_left;
		Frame T_right;

		T_right = Frame(Rotation::RotZ(0), Vector());
		T_left = Frame(Rotation::RotZ(0),Vector());
		// right_eye = head_T6
		right_eye_2_world_frame = head_end_frame * right_eye_offset * T_right;

		// left_eye = head_T6
		left_eye_2_world_frame = head_end_frame * left_eye_offset * T_left;

		//Get eye down to base of head
		/*if (torso)
		{
		  right_eye_2_head_base_frame=  torso_end_frame.M.Inverse()*right_eye_2_world_frame;
		  left_eye_2_head_base_frame=  torso_end_frame.M.Inverse()*left_eye_2_world_frame;
		}
		else
		{
		  right_eye_2_head_base_frame=right_eye_2_world_frame;
		  left_eye_2_head_base_frame=left_eye_2_world_frame;
		}*/

	}

///////////////////////////////////////////////////////////////
////   Copy the tranformed values into humanoid status
///////////////////////////////////////////////////////////////
	for (int i=0; i<6; i++)
	{
		if(right_arm && right_arm->GetDynamatics())
		{
			for (int j=0; j<right_arm->GetDynamatics()->GetNumDof(); j++)
				status.mutable_right_arm()->set_j((i*right_arm->GetDynamatics()->GetNumDof())+(j), J_right_arm(i,j));
			if(i<3)
				status.mutable_right_arm()->set_end_twist(i,right_arm_twist.vel[i]);
			else
				status.mutable_right_arm()->set_end_twist(i,right_arm_twist.rot[i-3]);
		}
		if(left_arm && left_arm->GetDynamatics())
		{
			for (int j=0; j<left_arm->GetDynamatics()->GetNumDof(); j++)
				status.mutable_left_arm()->set_j((i*left_arm->GetDynamatics()->GetNumDof())+(j), J_left_arm(i,j));
			if(i<3)
				status.mutable_left_arm()->set_end_twist(i,left_arm_twist.vel[i]);
			else
				status.mutable_left_arm()->set_end_twist(i,left_arm_twist.rot[i-3]);
		}
		if(head && head->GetDynamatics())
		{
			for (int j=0; j<head->GetDynamatics()->GetNumDof(); j++)
				status.mutable_head()->set_j((i*head->GetDynamatics()->GetNumDof())+(j), J_head(i,j));
			if(i<3)
				status.mutable_head()->set_end_twist(i,head_twist.vel[i]);
			else
				status.mutable_head()->set_end_twist(i,head_twist.rot[i-3]);
		}
		if(torso && torso->GetDynamatics())
		{
			for (int j=0; j<torso->GetDynamatics()->GetNumDof(); j++)
				status.mutable_torso()->set_j((i*torso->GetDynamatics()->GetNumDof())+(j), J_torso(i,j));
			if(i<3)
				status.mutable_torso()->set_end_twist(i,torso_twist.vel[i]);
			else
				status.mutable_torso()->set_end_twist(i,torso_twist.rot[i-3]);
		}
	}
	for (int i=0; i<3; i++)
	{
		if(right_arm && right_arm->GetDynamatics())
			status.mutable_right_arm()->set_end_pos(i,right_arm_end_frame.p[i]);
		if(left_arm && left_arm->GetDynamatics())
			status.mutable_left_arm()->set_end_pos(i,left_arm_end_frame.p[i]);
		if(torso && torso->GetDynamatics())
			status.mutable_torso()->set_end_pos(i,torso_end_frame.p[i]);
		if(head && head->GetDynamatics())
		{
			status.set_right_eye_pos(i, right_eye_2_world_frame.p[i]);
			status.set_left_eye_pos(i, left_eye_2_world_frame.p[i]);
			status.mutable_head()->set_end_pos(i,head_end_frame.p[i]);
		}
	}
	for (int i=0; i<9; i++)
	{
		if(right_arm && right_arm->GetDynamatics())
			status.mutable_right_arm()->set_end_rot(i,right_arm_end_frame.M.data[i]);
		if(left_arm && left_arm->GetDynamatics())
			status.mutable_left_arm()->set_end_rot(i,left_arm_end_frame.M.data[i]);
		if(torso && torso->GetDynamatics())
			status.mutable_torso()->set_end_rot(i,torso_end_frame.M.data[i]);
		if(head && head->GetDynamatics())
		{
			status.set_right_eye_rot(i, right_eye_2_world_frame.M.data[i]);
			status.set_left_eye_rot(i, left_eye_2_world_frame.M.data[i]);
			status.mutable_head()->set_end_rot(i,head_end_frame.M.data[i]);
		}
	}

/////////////////////////////////////////////////////
// Add wrenches to torso from child chains
////////////////////////////////////////////////////
// The dynamic payloads from arms, head are found using the arm and head dynamatics component to generate equivalent
// wrenches at each components T0 Frame.
// A dummy joint is assigned at T0 in X, Y, and Z direction and the torque values there are the
// torque component for that chain's wrench at T0.  The force component is mass*gravity.  This wrench gets transformed
// to Torso_T4 and added as an external wrench for the torso's dynamatics component.
///////////////////////////////////////////////////////

	if (torso && torso->GetDynamatics())
	{
		torso_wrench = Wrench();  // init to Zero
		// Transforming wrenches to Torso_T4
		if(right_arm && right_arm->GetDynamatics())
			torso_wrench+=right_arm_offset*right_arm->GetDynamatics()->GetBaseWrench();
		/*M3_INFO("Right arm: %f %f %f     %f %f %f\n",
			torso_wrench[0],
			torso_wrench[1],
			torso_wrench[2],
			torso_wrench[3],
			torso_wrench[4],
			torso_wrench[5]);*/
		if(left_arm && left_arm->GetDynamatics())
			torso_wrench+=left_arm_offset*left_arm->GetDynamatics()->GetBaseWrench();

		if(head && head->GetDynamatics())
			torso_wrench+=head_offset*head->GetDynamatics()->GetBaseWrench();

		torso->GetDynamatics()->SetEndWrench(torso_wrench);
	}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

bool M3Humanoid::ReadConfig(const char * filename)
{
	if (!M3Robot::ReadConfig(filename))
		return false;

	//YAML::Node doc;
	//GetYamlDoc(filename, doc);
	assert(doc.size());
#ifdef YAMLCPP_03
	const YAML::Node * ra_node;
	const YAML::Node * la_node;
	const YAML::Node * t_node;
	const YAML::Node * h_node;
	const YAML::Node * lh_node;
	const YAML::Node * rh_node;

	try {
		ra_node = &(doc["chains"]["right_arm"]);
	} catch(YAML::KeyNotFound& e) {
		ra_node = NULL;
	}

	try {
		la_node = &(doc["chains"]["left_arm"]);
	} catch(YAML::KeyNotFound& e) {
		la_node = NULL;
	}

	try {
		t_node = &(doc["chains"]["torso"]);
	} catch(YAML::KeyNotFound& e) {
		t_node = NULL;
	}

	try {
		h_node = &(doc["chains"]["head"]);
	} catch(YAML::KeyNotFound& e) {
		h_node = NULL;
	}
	try {
		rh_node = &(doc["chains"]["right_hand"]);
	} catch(YAML::KeyNotFound& e) {
		rh_node = NULL;
	}
	try {
		lh_node = &(doc["chains"]["left_hand"]);
	} catch(YAML::KeyNotFound& e) {
		lh_node = NULL;
	}
#endif
	vector<mReal> ra_rot;
	vector<mReal> la_rot;
	vector<mReal> t_rot;
	vector<mReal> h_rot;
	vector<mReal> rh_rot;
	vector<mReal> lh_rot;
	vector<mReal> ra_trans;
	vector<mReal> la_trans;
	vector<mReal> t_trans;
	vector<mReal> h_trans;
	/*vector<mReal> rh_trans;
	vector<mReal> lh_trans;*/
#ifdef YAMLCPP_03
	if (ra_node)
	{
		(*ra_node)["chain_component"] >> right_arm_name;
		ra_rot = YamlReadVectorM((*ra_node)["base_rotation_in_parent"]);
		ra_trans = YamlReadVectorM((*ra_node)["base_translation_in_parent"]);
		try {
		  (*ra_node)["force_shm_mode"] >> force_shm_r_arm;
		} catch(YAML::KeyNotFound& e) {
			force_shm_r_arm = false;
		}
		try{
		  (*ra_node)["use_loadx6_instead_of_payload"]>>right_use_loadx6_instead_of_payload;
		}
		catch(YAML::KeyNotFound& e)
		{
		  right_use_loadx6_instead_of_payload = false;
		}

		try{
		  (*ra_node)["load_x6_component"]>>right_load_x6_component;
		}
		catch(YAML::KeyNotFound& e)
		{
		  right_load_x6_component = "";
		}
	}
	if (la_node)
	{
		(*la_node)["chain_component"] >> left_arm_name;
		la_rot = YamlReadVectorM((*la_node)["base_rotation_in_parent"]);
		la_trans = YamlReadVectorM((*la_node)["base_translation_in_parent"]);
		try {
		  (*la_node)["force_shm_mode"] >> force_shm_l_arm;
		} catch(YAML::KeyNotFound& e) {
			force_shm_l_arm = false;
		}
	}
	if (t_node)
	{
		(*t_node)["chain_component"] >> torso_name;
		t_rot = YamlReadVectorM((*t_node)["base_rotation_in_parent"]);
		t_trans = YamlReadVectorM((*t_node)["base_translation_in_parent"]);
		try {
		  (*t_node)["force_shm_mode"] >>force_shm_torso;
		} catch(YAML::KeyNotFound& e) {
			force_shm_torso = false;
		}
	}
	if (h_node)
	{
		(*h_node)["chain_component"] >> head_name;
		h_rot = YamlReadVectorM((*h_node)["base_rotation_in_parent"]);
		h_trans = YamlReadVectorM((*h_node)["base_translation_in_parent"]);
		try {
		  (*h_node)["force_shm_mode"] >>force_shm_head;
		} catch(YAML::KeyNotFound& e) {
			force_shm_head = false;
		}

	}
	if (rh_node)
	{
		(*rh_node)["chain_component"] >> right_hand_name;
		//rh_rot = YamlReadVectorM((*rh_node)["base_rotation_in_parent"]);
		//rh_trans = YamlReadVectorM((*rh_node)["base_translation_in_parent"]);
		try {
		  (*rh_node)["force_shm_mode"] >>force_shm_r_hand;
		} catch(YAML::KeyNotFound& e) {
			force_shm_r_hand = false;
		}

	}
	if (lh_node)
	{
		(*lh_node)["chain_component"] >> left_hand_name;
		//lh_rot = YamlReadVectorM((*lh_node)["base_rotation_in_parent"]);
		//lh_trans = YamlReadVectorM((*lh_node)["base_translation_in_parent"]);
		try {
		  (*lh_node)["force_shm_mode"] >>force_shm_l_hand;
		} catch(YAML::KeyNotFound& e) {
			force_shm_l_hand = false;
		}

	}
#else
	if (doc["chains"]["right_arm"])
	{
		YAML::Node ra_node = doc["chains"]["right_arm"];
		(ra_node)["chain_component"] >> right_arm_name;
		(ra_node)["base_rotation_in_parent"] >> ra_rot;
		(ra_node)["base_translation_in_parent"] >> ra_trans;
		try {
		  (ra_node)["force_shm_mode"] >> force_shm_r_arm;
		} catch(YAML::Exception& e) {
			force_shm_r_arm = false;
		}
		try{
		  (ra_node)["use_loadx6_instead_of_payload"]>>right_use_loadx6_instead_of_payload;
		}
		catch(YAML::Exception& e)
		{
		  right_use_loadx6_instead_of_payload = false;
		}

		try{
		  (ra_node)["load_x6_component"]>>right_load_x6_component;
		}
		catch(YAML::Exception& e)
		{
		  right_load_x6_component = "";
		}
	}
	if (doc["chains"]["left_arm"])
	{
		YAML::Node la_node = doc["chains"]["left_arm"];
		(la_node)["chain_component"] >> left_arm_name;
		(la_node)["base_rotation_in_parent"] >> la_rot;
		(la_node)["base_translation_in_parent"] >> la_trans;
		try {
		  (la_node)["force_shm_mode"] >> force_shm_l_arm;
		} catch(YAML::Exception& e) {
			force_shm_l_arm = false;
		}
	}
	if (doc["chains"]["torso"])
	{
		YAML::Node t_node = doc["chains"]["torso"];
		(t_node)["chain_component"] >> torso_name;
		(t_node)["base_rotation_in_parent"] >> t_rot;
		(t_node)["base_translation_in_parent"] >> t_trans;
		try {
		  (t_node)["force_shm_mode"] >>force_shm_torso;
		} catch(YAML::Exception& e) {
			force_shm_torso = false;
		}
	}
	if (doc["chains"]["head"])
	{
		YAML::Node h_node = doc["chains"]["head"];
		(h_node)["chain_component"] >> head_name;
		(h_node)["base_rotation_in_parent"] >> h_rot;
		(h_node)["base_translation_in_parent"] >> h_trans;
		try {
		  (h_node)["force_shm_mode"] >>force_shm_head;
		} catch(YAML::Exception& e) {
			force_shm_head = false;
		}

	}
	if (doc["chains"]["right_hand"])
	{
		YAML::Node rh_node = doc["chains"]["right_hand"];
		(rh_node)["chain_component"] >> right_hand_name;
		//(rh_node)["base_rotation_in_parent"] >> rh_rot;
		//(rh_node)["base_translation_in_parent"] >> rh_trans;
		try {
		  (rh_node)["force_shm_mode"] >>force_shm_r_hand;
		} catch(YAML::Exception& e) {
			force_shm_r_hand = false;
		}

	}
	if (doc["chains"]["left_hand"])
	{
		YAML::Node lh_node = doc["chains"]["left_hand"];
		(lh_node)["chain_component"] >> left_hand_name;
		//(lh_node)["base_rotation_in_parent"] >> lh_rot;
		//(lh_node)["base_translation_in_parent"] >> lh_trans;
		try {
		  (lh_node)["force_shm_mode"] >>force_shm_l_hand;
		} catch(YAML::Exception& e) {
			force_shm_l_hand = false;
		}

	}
#endif
	try{
	  doc["startup_motor_pwr_on"]>>startup_motor_pwr_on;
	}
	catch(...)
	{
	  startup_motor_pwr_on=false;
	}

	Rotation ra_rot_kdl;
	Vector ra_vec_kdl;
	Rotation la_rot_kdl;
	Vector la_vec_kdl;
	Rotation t_rot_kdl;
	Vector t_vec_kdl;
	Rotation h_rot_kdl;
	Vector h_vec_kdl;
	/*Rotation rh_rot_kdl;
	Vector rh_vec_kdl;
	Rotation lh_rot_kdl;
	Vector lh_vec_kdl;*/

	for(int i=0;i<ra_rot.size();i++) ra_rot_kdl.data[i] = ra_rot[i];
	for(int i=0;i<ra_trans.size();i++) ra_vec_kdl[i] = ra_trans[i];
	for(int i=0;i<la_rot.size();i++) la_rot_kdl.data[i] = la_rot[i];
	for(int i=0;i<la_trans.size();i++) la_vec_kdl[i] = la_trans[i];
	for(int i=0;i<t_rot.size();i++) t_rot_kdl.data[i] = t_rot[i];
	for(int i=0;i<t_trans.size();i++) t_vec_kdl[i] = t_trans[i];
	for(int i=0;i<h_rot.size();i++) h_rot_kdl.data[i] = h_rot[i];
	for(int i=0;i<h_trans.size();i++) h_vec_kdl[i] = h_trans[i];
	/*for(int i=0;i<rh_rot.size();i++) rh_rot_kdl.data[i] = rh_rot[i];
	for(int i=0;i<rh_trans.size();i++) rh_vec_kdl[i] = rh_trans[i];
	for(int i=0;i<lh_rot.size();i++) lh_rot_kdl.data[i] = lh_rot[i];
	for(int i=0;i<lh_trans.size();i++) lh_vec_kdl[i] = lh_trans[i];*/

	right_arm_offset=Frame(ra_rot_kdl, ra_vec_kdl);
	left_arm_offset=Frame(la_rot_kdl, la_vec_kdl);
	torso_offset=Frame(t_rot_kdl, t_vec_kdl);

	head_offset=Frame(h_rot_kdl, h_vec_kdl);

	return true;
}

M3BaseHumanoidCommand * M3Humanoid::GetCmd(M3Chain chain)
{
  switch (chain)
  {
    case RIGHT_ARM:
      return command.mutable_right_arm();
    case LEFT_ARM:
      return command.mutable_left_arm();

    case TORSO:
      return command.mutable_torso();
    case HEAD:
      return command.mutable_head();
    case LEFT_HAND:
	    return command.mutable_left_hand();
    case RIGHT_HAND:
	    return command.mutable_right_hand();
  }
  return NULL;
}

const M3BaseHumanoidStatus * M3Humanoid::GetStatus(M3Chain chain)
{
  switch (chain)
  {
    case RIGHT_ARM:
      return &status.right_arm();
    case LEFT_ARM:
      return &status.left_arm();
    case TORSO:
      return &status.torso();
    case HEAD:
      return &status.head();
    case LEFT_HAND:
	    return &status.left_hand();
    case RIGHT_HAND:
	    return &status.right_hand();
  }
  return NULL;

}

M3BaseHumanoidParam * M3Humanoid::GetParam(M3Chain chain)
{
  switch (chain)
  {
    case RIGHT_ARM:
      return param.mutable_right_arm();
    case LEFT_ARM:
      return param.mutable_left_arm();
    case TORSO:
      return param.mutable_torso();
    case HEAD:
      return param.mutable_head();
    case LEFT_HAND:
      return param.mutable_left_hand();
    case RIGHT_HAND:
      return param.mutable_right_hand();
  }
  return NULL;

}

void M3Humanoid::SetThetaDeg(M3Chain chain,unsigned int idx, mReal theta)
{
   M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->q_desired_size())
     cmd->set_q_desired(idx,theta);
}

mReal M3Humanoid::GetThetaDeg(M3Chain chain,unsigned int  idx)
{
  const M3BaseHumanoidStatus * status = GetStatus(chain);
   if (status == NULL)
     return 0;
   if (idx < status->theta_size())
     return status->theta(idx);
   else
     return 0;
}

JOINT_ARRAY_MODE M3Humanoid::GetMode(M3Chain chain,unsigned int  idx)
{
 M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return JOINT_ARRAY_MODE_OFF;
   if (idx < cmd->ctrl_mode_size())
     return cmd->ctrl_mode(idx);
   else
     return JOINT_ARRAY_MODE_OFF;
}

mReal M3Humanoid::GetThetaDotDeg(M3Chain chain,unsigned int  idx)
{
  const M3BaseHumanoidStatus * status = GetStatus(chain);
   if (status == NULL)
     return 0;
   if (idx < status->thetadot_size())
     return status->thetadot(idx);
   else
     return 0;
}

mReal M3Humanoid::GetThetaDotDotDeg(M3Chain chain,unsigned int  idx)
{
  const M3BaseHumanoidStatus * status = GetStatus(chain);
   if (status == NULL)
     return 0;
   if (idx < status->thetadotdot_size())
     return status->thetadotdot(idx);
   else
     return 0;
}

mReal M3Humanoid::GetTorque_mNm(M3Chain chain,unsigned int  idx)
{
  const M3BaseHumanoidStatus * status = GetStatus(chain);
   if (status == NULL)
     return 0;
   if (idx < status->torque_size())
     return status->torque(idx);
   else
     return 0;
}

mReal M3Humanoid::GetTorqueDot_mNm(M3Chain chain,unsigned int  idx)
{
  const M3BaseHumanoidStatus * status = GetStatus(chain);
   if (status == NULL)
     return 0;
   if (idx < status->torquedot_size())
     return status->torquedot(idx);
   else
     return 0;
}

mReal M3Humanoid::GetGravity(M3Chain chain,unsigned int  idx)
{
  const M3BaseHumanoidStatus * status = GetStatus(chain);
   if (status == NULL)
     return 0;
   if (idx < status->g_size())
     return status->g(idx);
   else
     return 0;
}

mReal M3Humanoid::GetPwm(M3Chain chain,unsigned int  idx)
{
  const M3BaseHumanoidStatus * status = GetStatus(chain);
   if (status == NULL)
     return 0;
   if (idx < status->pwm_cmd_size())
     return status->pwm_cmd(idx);
   else
     return 0;
}

mReal M3Humanoid::GetPayloadMass(M3Chain chain)
{
  M3BaseHumanoidParam * param = GetParam(chain);
   if (param == NULL)
     return 0;

   return param->payload_mass();
}

mReal M3Humanoid::GetPayloadInertia(M3Chain chain,unsigned int  idx)
{
  M3BaseHumanoidParam * param = GetParam(chain);
   if (param == NULL)
     return 0;
   if (idx < param->payload_inertia_size())
     return param->payload_inertia(idx);
   else
     return 0;
}

mReal M3Humanoid::GetPayloadCom(M3Chain chain,unsigned int  idx)
{
  M3BaseHumanoidParam * param = GetParam(chain);
   if (param == NULL)
     return 0;
   if (idx < param->payload_com_size())
     return param->payload_com(idx);
   else
     return 0;
}

void M3Humanoid::SetTorque_mNm(M3Chain chain,unsigned int idx, mReal torque)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->tq_desired_size())
     cmd->set_tq_desired(idx, torque);
}

void M3Humanoid::SetThetaSharedMem_Deg(M3Chain chain,unsigned int idx, mReal theta)
{
  switch (chain)
  {
    case HEAD:
      if (idx < angle_shm_head.rows())
	angle_shm_head(idx) = theta;
      break;
  }
}

void M3Humanoid::SetSlewRateSharedMem_Deg(M3Chain chain,unsigned int idx, mReal theta)
{
  switch (chain)
  {
    case HEAD:
      if (idx < angle_shm_head.rows())
	slew_rate_shm_head(idx) = theta;
      break;
  }
}

void M3Humanoid::SetTorqueSharedMem_mNm(M3Chain chain,unsigned int idx, mReal torque)
{
  switch (chain)
  {
    case RIGHT_ARM:
      if (idx < torque_shm_right_arm.rows())
	torque_shm_right_arm(idx) = torque;
      break;
    case LEFT_ARM:
      if (idx < torque_shm_left_arm.rows())
	torque_shm_left_arm(idx) = torque;
      break;
    case TORSO:
      if (idx < torque_shm_torso.rows())
	torque_shm_torso(idx) = torque;
      break;
   case RIGHT_HAND:
      if (idx < torque_shm_right_hand.rows())
	torque_shm_right_hand(idx) = torque;
      break;
   case LEFT_HAND:
      if (idx < torque_shm_left_hand.rows())
	torque_shm_left_hand(idx) = torque;
      break;
  }
}

void M3Humanoid::SetThetaDotDeg(M3Chain chain,unsigned int idx, mReal theta_dot)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->qdot_desired_size())
     cmd->set_qdot_desired(idx, theta_dot);
}

void M3Humanoid::SetPwm(M3Chain chain,unsigned int idx, mReal pwm)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->pwm_desired_size())
     cmd->set_pwm_desired(idx, pwm);
}

void M3Humanoid::SetSlewRate(M3Chain chain,unsigned int idx, mReal slew_rate)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->q_slew_rate_size())
     cmd->set_q_slew_rate(idx, slew_rate);
}

void M3Humanoid::SetSlewRateProportional(M3Chain chain,unsigned int idx, mReal slew_rate)
{
  mReal max_slew;
  switch(chain)
  {
    case RIGHT_ARM:
      max_slew = ((M3JointParam*)right_arm->GetJoint(idx)->GetParam())->max_q_slew_rate();
      break;
    case LEFT_ARM:
      max_slew = ((M3JointParam*)left_arm->GetJoint(idx)->GetParam())->max_q_slew_rate();
      break;
    case TORSO:
      max_slew = ((M3JointParam*)torso->GetJoint(idx)->GetParam())->max_q_slew_rate();
      break;
    case HEAD:
      max_slew = ((M3JointParam*)head->GetJoint(idx)->GetParam())->max_q_slew_rate();
      break;
    case LEFT_HAND:
      max_slew = ((M3JointParam*)left_hand->GetJoint(idx)->GetParam())->max_q_slew_rate();
      break;
    case RIGHT_HAND:
      max_slew = ((M3JointParam*)right_hand->GetJoint(idx)->GetParam())->max_q_slew_rate();
      break;
  }

  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->q_slew_rate_size())
     cmd->set_q_slew_rate(idx, CLAMP(slew_rate, 0.0, 1.0)*max_slew);
}

void M3Humanoid::SetModePwm(M3Chain chain,unsigned int  idx)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->ctrl_mode_size())
     cmd->set_ctrl_mode(idx, JOINT_ARRAY_MODE_PWM);
}

void M3Humanoid::SetModeTorque(M3Chain chain,unsigned int  idx)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->ctrl_mode_size())
     cmd->set_ctrl_mode(idx, JOINT_ARRAY_MODE_TORQUE);
}

void M3Humanoid::SetModeTorqueGc(M3Chain chain,unsigned int  idx)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->ctrl_mode_size())
     cmd->set_ctrl_mode(idx, JOINT_ARRAY_MODE_TORQUE_GC);
}

void M3Humanoid::SetModeThetaGcMj(M3Chain chain,unsigned int  idx)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->ctrl_mode_size())
     cmd->set_ctrl_mode(idx, JOINT_ARRAY_MODE_THETA_GC);
}

void M3Humanoid::SetModeThetaMj(M3Chain chain,unsigned int  idx)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->ctrl_mode_size())
     cmd->set_ctrl_mode(idx, JOINT_ARRAY_MODE_THETA_MJ);
}

void M3Humanoid::SetModeThetaGc(M3Chain chain,unsigned int  idx)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->ctrl_mode_size())
     cmd->set_ctrl_mode(idx, JOINT_ARRAY_MODE_THETA_GC);
}
void M3Humanoid::SetModeThetaDotGc(M3Chain chain,unsigned int  idx)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->ctrl_mode_size())
     cmd->set_ctrl_mode(idx, JOINT_ARRAY_MODE_THETADOT_GC);
}
void M3Humanoid::SetModeThetaDot(M3Chain chain,unsigned int  idx)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->ctrl_mode_size())
     cmd->set_ctrl_mode(idx, JOINT_ARRAY_MODE_THETADOT);
}
void M3Humanoid::SetModeTheta(M3Chain chain,unsigned int  idx)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->ctrl_mode_size())
     cmd->set_ctrl_mode(idx, JOINT_ARRAY_MODE_THETA);
}

void M3Humanoid::SetModeOff(M3Chain chain,unsigned int  idx)
{
  M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->ctrl_mode_size())
     cmd->set_ctrl_mode(idx, JOINT_ARRAY_MODE_OFF);
}

void M3Humanoid::SetStiffness(M3Chain chain,unsigned int  idx, mReal stiffness)
{
   M3BaseHumanoidCommand * cmd = GetCmd(chain);
   if (cmd == NULL)
     return;
   if (idx < cmd->q_stiffness_size())
     cmd->set_q_stiffness(idx, stiffness);
}

void M3Humanoid::SetPayloadMass(M3Chain chain, mReal mass)
{
   M3BaseHumanoidParam * param = GetParam(chain);
   if (param == NULL)
     return;
  param->set_payload_mass(mass);

}

void M3Humanoid::SetPayloadInertia(M3Chain chain,unsigned int  idx, mReal inertia)
{
   M3BaseHumanoidParam * param = GetParam(chain);
   if (param == NULL)
     return;
   if (idx < param->payload_inertia_size())
    param->set_payload_inertia(idx, inertia);
}

void M3Humanoid::SetPayloadCom(M3Chain chain,unsigned int  idx, mReal com)
{
   M3BaseHumanoidParam * param = GetParam(chain);
   if (param == NULL)
     return;
    if (idx < param->payload_com_size())
      param->set_payload_com(idx, com);
}

Eigen::Vector3d M3Humanoid::GetEndPosition(M3Chain chain)
{
  Eigen::Vector3d end_pos;
  const M3BaseHumanoidStatus * status = GetStatus(chain);
  for (int i = 0; i<3; i++)
    end_pos[i] = status->end_pos(i);

  return end_pos;
}

Eigen::Matrix3d M3Humanoid::GetEndRotation(M3Chain chain)
{
  Eigen::Matrix3d end_rot;
  const M3BaseHumanoidStatus * status = GetStatus(chain);
  for (int i = 0; i<3; i++)
  {
    for (int j = 0; j<3; j++)
      end_rot(i,j) = status->end_rot((i*3)+(j));
  }
  return end_rot;
}

Eigen::MatrixXd M3Humanoid::GetJacobian(M3Chain chain)
{
  const M3BaseHumanoidStatus * status = GetStatus(chain);
  Eigen::MatrixXd jac = Eigen::MatrixXd::Zero(6,GetNdof(chain));

  for (int i = 0; i<6; i++)
  {
    for (int j = 0; j<GetNdof(chain); j++)
      jac(i,j) = status->j((i*GetNdof(chain))+(j));
  }
  return jac;

}

int M3Humanoid::GetNdof(M3Chain chain)
{
    switch(chain)
    {
      case RIGHT_ARM:
	if (right_arm)
	  return right_arm->GetNumDof();
	break;
      case LEFT_ARM:
	if (left_arm)
	  return left_arm->GetNumDof();
	break;
      case TORSO:
	if (torso)
	  return torso->GetNumDof();
	break;
      case HEAD:
	if (head)
	  return head->GetNumDof();
	break;
      case LEFT_HAND:
	if (left_hand)
	  return left_hand->GetNumDof();
	break;
      case RIGHT_HAND:
	if (right_hand)
	  return right_hand->GetNumDof();
	break;
    }
    return 0;
}

void M3Humanoid::SetMotorPowerOn()
{
    command.set_enable_motor(true);
}

void M3Humanoid::SetMotorPowerOff()
{
    command.set_enable_motor(false);
}

void M3Humanoid::TestAPI()
{
  SetMotorPowerOn();

  SetModeOff(RIGHT_ARM,0);
  SetModePwm(RIGHT_ARM,0);
  SetModeTorque(RIGHT_ARM,0);
  SetModeThetaGcMj(RIGHT_ARM,0);
  SetModeThetaMj(RIGHT_ARM,0);
  SetModeTheta(RIGHT_ARM,0);
  SetModeThetaGc(RIGHT_ARM,0);
  SetThetaDeg(RIGHT_ARM,0,20);
  SetStiffness(RIGHT_ARM,0,.7);
  M3_INFO("t: %f\n",GetThetaDeg(RIGHT_ARM, 0));
  M3_INFO("td: %f\n",GetThetaDotDeg(RIGHT_ARM, 0));
  M3_INFO("tdd: %f\n",GetThetaDotDotDeg(RIGHT_ARM, 0));
  M3_INFO("q: %f\n",GetTorque_mNm(RIGHT_ARM, 0));
  M3_INFO("qd: %f\n",GetTorqueDot_mNm(RIGHT_ARM, 0));
  Eigen::Vector3d ep = GetEndPosition(RIGHT_ARM);
  for (int i = 0; i++; i<3)
    M3_INFO("ep %i: %f\n",i,ep[i]);
  Eigen::Matrix3d er = GetEndRotation(RIGHT_ARM);
  for (int i = 0; i++; i<9)
    M3_INFO("er %i: %f\n",i,er(i));
  Eigen::MatrixXd j = GetJacobian(RIGHT_ARM);
  for (int i = 0; i++; i<(6*7))
    M3_INFO("j %i: %f\n",i,j(i));
  M3_INFO("g: %f\n",GetGravity(RIGHT_ARM, 0));
  M3_INFO("p: %f\n",GetPwm(RIGHT_ARM, 0));
  SetTorque_mNm(RIGHT_ARM,0,30);
  SetPwm(RIGHT_ARM,0,40);
  SetSlewRate(RIGHT_ARM,0,50);
  SetSlewRateProportional(RIGHT_ARM,0,.8);
  M3_INFO("m: %f\n",GetPayloadMass(RIGHT_ARM));
  M3_INFO("i: %f\n",GetPayloadInertia(RIGHT_ARM,0));
  M3_INFO("i: %f\n",GetPayloadCom(RIGHT_ARM,0));
  SetPayloadMass(RIGHT_ARM,2);
  SetPayloadCom(RIGHT_ARM,0,3);
  SetPayloadInertia(RIGHT_ARM,0,2);
  M3_INFO("dof: %d\n",GetNdof(RIGHT_ARM));

  SetMotorPowerOff();
}

}

redpajama_set_name: RedPajamaGithub


This website ("Website") is owned and provided by Gullivers Sports Travel. Gullivers Sports Travel Ltd. is a member of the Travelopia Group of companies (registered in England and Wales with company number 2746479) whose registered address is at Origin One, 108 High Street, Crawley, West Sussex, RH10 1BD and whose registered VAT number is GB 275 2107 71.
Our aim is at all times to provide you with an excellent service. If you are unhappy with our service for any reason, please contact us via our Contact Us page or by writing to us at Barmy Travel, c/o Gullivers Sports Travel, Ground Floor, Ashvale 2, Ashchurch Business Centre, Alexandra Way, Tewkesbury, Gloucestershire GL20 8NB.
Unless otherwise stated, we own (or are licensed to use) the intellectual property rights in the content and information in this Website, including (without limitation) all text, sound, photographs, images, logos, videos, 360° maps, podcasts, blogs, customer reviews, graphics, design, underlying source code and software. Subject to the "Non-Commercial Use Only" section above, material and information, either in whole or in part, from this Website may not be reproduced, copied, republished, downloaded, posted, broadcast or transmitted in any form or medium without our and/or the appropriate owner's prior written permission.


PDF Splitter + is a handy application which helps you easily and quickly split a large PDF files into individual PDF files, or extract pages from a PDF file in flexible ways. It is the best software to manage large PDF files that take extra time in loading or transferring.
It takes only 3 steps to get things done. Just simply add file, select a splitting method and click 'Extract' button.
3. Original formatting and structure of the content remains unchanged.
4. Quick and fast, splitting a large PDF file with over 600 pages in less than 1 mins.


Daniels tells Anderson Cooper that she spanked the future president with a magazine; he said he'd put her on "Celebrity Apprentice"; they spent their second date watching Shark Week; and she was threatened to stay quiet.
"So why didn't he? It just slipped his mind? It's just a coincidence that, in the waning days of the campaign, he thought to himself, 'Oh, you know, I know I've been thinkin' about this for years. Perhaps now is a good time to get that NDA executed with Stormy Daniels,'" Avenatti said.


Roblox Royale High Gameplay! Late for my NEW BAKING CLASS! BIG FAT F!
0:03:13 I sort of join in late at the end of the BAKING CLASS!
0:13:26 I made it to Art Class! I got an A!
0:18:40 I missed my English Class and got an F!
0:20:52 Checking out all the styles and moods!
0:24:38 Dressing up for Royal Dance and remembered I forgot to do my homework!
0:29:02 Voting time and I WON THE PROM QUEEN!
- New baking wings! Cakes, Candy, & Aprons!
- The new interface has been launched! Easily tuck it away for extra screen real estate ????
- New grading system: no more popups, easily see your friends grades if you are next to them ????


text:
Bush's last days
Many people have been forced into having their own verbal and intellectual lenses for explaining the behaviors of George Bush.
He can't just be insane, so — what is it? What explains his behavior?
Today, as Congress fished around for money to save the U.S. auto companies, in a pickle because they had not invested earlier in alternative – energy projects, George Bush gave a speech, suggesting that Congress take the money from alternative energy projects to prop up the dying carmakers.
Is he really an idiot?
Some day, in the not so distant future, several things may happen:
1. As laid out in Harper's this month, the President, the Real President (Cheney) et. al may face domestic or international criminal charges for war crimes.
2. Not that George can not pardon himself or others for international crimes, as in the Nurnburg Trial. Even if he escapes domestic prosecution, he may end up like Pinochet, hounded worldwide by courts elsewhere, kidnapped, dragged around, arrested and jailed elsehwhere, etc.
3. The simplest way to understand the entire Cheney/Bush regime is to assume that family ties to the Saudis were more important than catching bin Laden, that oil in general was more important than anything Bush swore to uphold on that Bible during his inauguration, and that Bush et. al (the heading on future lawsuits, ad infinitum) were embarked on an intentional, planned, consistent program of looting taxpayer monies for their personal and private benefit.
It is impossible to forget: We still have a month and more to go. There are plenty of miscreants on a master scale who would like to have a parting shot at screwing everything up for normal people in return for private gain, at the cost of a single large check to the Bush Library (Bush doesn't read books; what a joke).
Bend over, and Get ready. George never cared for us then, and he still doesn't today. He serves only his family, and a few close "friends."
Source: Mark Anderson.

redpajama_set_name: RedPajamaCommonCrawl


text:
COMM411: Public Relations
Unit 9: Careers in Public Relations
9.1: Job Outlook for Public Relations Professionals
Steven R. Van Hook's How-To Marketing: "Marketing Employment Trends"
Back to '9.1: Job Outlook for Public Relations Professionals'
Read this summary of the employment outlook in the fields of marketing, public relations, advertising, and sales.
Click https://web.archive.org/web/20160308212437/http://howtomarketing.us/vanhook3.htm link to open resource.
◄ U.S. Bureau of Labor Statistics: Occupational Outlook Handbook: "Public Relations Managers and Specialists"
Jump to... Jump to... Course Syllabus Course Terms of Use The Economist: "Rise of the Image Men" Mastering Public Relations: "Chapter 1: The Importance of Public Relations" Mastering Public Relations: "Chapter 2: What Is Public Relations?" Wikipedia: "Public Relations" Steve Simon's "Traditional PR Still Remains Strong" Peter Granat's "A Recipe for Marketing Success" Marsha Friedman's "The Past, Present and Future of Public Relations" Blake Miller's "The New Rules for Public Relations" Unit 1 Discussion Mastering Public Relations: "Chapter 3: Models and Approaches to Public Relations" Steven R. Van Hook's "Media and Public Relations: A Primer" Steven R. Van Hook's "Marketing Fundamentals" Craig Miyamoto's "How to Write a Comprehensive Public Relations Plan" Steve Le Vine's "Public Relations: Your Saving Grace?" Pari Noskin Taichert's "Plan It: A Four-Point PR Primer" Unit 2 Discussion Mastering Public Relations: "Chapter 7: Identifying and Prioritizing Stakeholders and Publics" Steven R. Van Hook's "Who Are Your Publics?" Steven R. Van Hook's "Marketing Research Resources" Formulating Messages for Diverse Demographics Steven R. Van Hook's "Transcultural Communications" Unit 3 Discussion Steven R. Van Hook's Media and Public Relations Primer: "Module 3: Understanding the Media" Media and Technology Attracting Media Attention Steven R. Van Hook's "Communication Appeals and Tactics" Media Releases Steven R. Van Hook's Media and Public Relations Primer: "Module 4: Effective Media Releases, Kits, and Conferences" Pitching Stories Presenting Yourself Steven R. Van Hook's Media and Public Relations Primer: "Module 5: Preparing for Media Interviews" Maintaining Media Relationships Unit 4 Discussion Mastering Public Relations: "Chapter 8: Public Relations Research: The Key to Strategy" The Associated Press Stylebook Steven R. Van Hook's "Best Writing Tips" Improving Your Writing Creating PR Materials Public Speaking Mastering Public Relations: "Chapter 5: Organizational Factors for Excellent Public Relations" and "Chapter 6: Public Relations and Organizational Effectiveness" All About Public Relations Unit 5 Discussion Nedra Kline Weinreich's "What Is Social Marketing?" Steven R. Van Hook's "Social Marketing" Mastering Public Relations: "Chapter 10: The Practice of Public Relations" Methods of Social Public Relations Connecting with Social Programs Steven R. Van Hook's How-To Marketing: "Social Marketing" Unit 6 Discussion Crisis Events and Plan Preparation How to Avoid Crises Richard S. Levick's "The Art of Mea Culpa" How to Fix Crises Case Studies Unit 7 Discussion Codes of Ethics Illinois Institute of Technology, Center for the Study of Ethics in the Professions Mastering Public Relations: "Chapter 11: Ethics, Leadership and Counseling, and Moral Analyses" and "Chapter 12: Best Practices for Excellence in Public Relations" Steven Le Vine's "How to Run Ethically Sound PR Campaigns" The Center for Media and Democracy: "PR Watch" Unit 8 Discussion U.S. Bureau of Labor Statistics: Occupational Outlook Handbook: "Public Relations Managers and Specialists" Just Starting Out Public Relations Internships Internship Programs Entry-Level PR Jobs Career Advancement Public Relations Society of America, Job Center Steven R. Van Hook's "Your Future in Public Relations" Database of Transnational Organizations Unit 9 Discussion Council of Public Relations Firms International Association of Business Communicators International Public Relations Association Public Relations Society of America Forming Professional Social Networks Unit 10 Discussion Course Feedback Survey COMM411: Certificate Final Exam
Just Starting Out ►
Unit 1: Foundations of Public Relations
Unit 2: Public Relations Components and Planning
Unit 3: Key Target Audiences
Unit 4: Media Relations
Unit 5: Writing for Public Relations
Unit 6: Social Projects and Public Relations
Unit 7: Crisis Communications
Unit 8: Ethical Issues in Public Relations
Unit 10: Professional Development


text:
Q: Weblogic EAR Classloading I am deploying an EAR in a WebLogic node with many jars defined in the bootstrap (startWeblogicServer.bat) class-path.  The problem is that my ear and the bootstrap contain different versions of the same jars, not only that but certain jars contain extracted  third party libraries which also differ in version from the WebLogic bootstrap jars causing all kinds of classpath errors.
I know you can set preferred jars in the EAR application xml but, this can be very tedious to resolve with regards to jars which include extracted third party libraries in terms of understanding all the dependencies..
Is there a correct approach that i need to be taking here?  Am i thinking about this in the wrong way?  Any help would be greatly appreciated!
So far prefer-web-inf-classes has been recommended but wont work because i'm not deploying a WAR, also prefer-application-packages is what we are currently using but still has the issue described above... Anymore advice out there??  Thanks!

A: I think you should remove the JARs from the server bootstrap area and let each domain load the JARs from their individual EAR using the domain level class loader.
You already know this is true, because the approach you're taking is giving you problems.
The only justification for doing it your way is to try and save some disk space by not duplicating JARs in multiple domains.  I say that disk space is cheap, and getting cheaper all the time.  Common JARs mean common dependencies, and you don't want to force every domain on a WebLogic instance to have to upgrade a JAR just because one citizen needs it.
Remove the JARs from the server bootstrap and duplicate the ones you need in each EAR.  Make each EAR, each domain independent of the others.
UPDATE:
You need to get your own server.
Or add the <prefer-web-inf-classes> tag to your configuration.
Consult this for details.

redpajama_set_name: RedPajamaStackExchange


1)married children towards old aged parents?
2)son in law and daughter in laws towards mother in law and father in law?
1) The son should aid as per his financial ability.
2) Be good, cordial and if there is need, helpful.


If you paid attention to the MLB Home Run Derby a few weeks ago, you might have seen something pretty crazy. Yankees outfielder Aaron Judge stepped up to the plate and banged a homer so hard that it flew all the way up to the ceiling of Marlins Park and bounced off.
This is supposed to be impossible. Stadium designers work extremely hard to ensure that all of their park's features are well out of reach of errant balls, both to preserve the integrity of the game and to prevent damage. But Judge is no ordinary hitter. Earlier in July, he smashed a homer so hard that it left a dent in the wall of Yankee Stadium. This dude swings a bat really hard.
Here's the video of his Home Run Derby smash so you can see it with your own eyes. Watch that sucker fly!
Wired has an explanation of the physics behind the crazy hit. It examines the force that Judge swings his bat as well as the advantageous angle that he makes contact. In short, this was an incredibly unlikely hit, but as batters get more and more powerful it's the kind of thing we might start seeing more often. Good luck catching one of these babies if it goes into the stands without breaking a few fingers.


Chitralekh, S and Avudainayagam, KV and Pappu, SV (1989) Rotation sensitivity of Lau fringes: an analysis based on coherence theory. In: Optics & Laser Technology, 21 (4). pp. 265-267.
An analysis based on coherence theory is presented, which explains the experimentally observed rotation sensitivity of the contrast of Lau fringes obtained under spatially incoherent illumination.


text:
Home › Family Ethics
Family Ethics
Moral Traditions series
2018CTSA
Q&A: Julie Rubio on Family Ethics
Practices for Christians
Julie Hanlon Rubio
Winner of the 2011 Catholic Press Association Book Award for Family Life
How can ordinary Christians find moral guidance for the mundane dilemmas they confront in their daily lives? To answer this question, Julie Hanlon Rubio brings together a rich Catholic theology of marriage and a strong commitment to social justice to focus on the place where the ethics of ordinary life are played out: the family.
Sex, money, eating, spirituality, and service. According to Rubio, all are areas for practical application of an ethics of the family. In each area, intentional practices can function as acts of resistance to a cultural and middle-class conformity that promotes materialism over relationships. These practices forge deep connections within the family and help families live out their calling to be in solidarity with others and participate in social change from below. It is through these everyday moral choices that most Christians can live out their faith—and contribute to progress in the world.
Julie Hanlon Rubio is associate professor of Christian ethics at Saint Louis University. She is the author of A Christian Theology of Marriage and Family and coeditor (with Charles E. Curran) of Readings in Moral Theology No. 15. Rubio lives in St. Louis with her husband and their three sons.
James F. Keenan, SJ, Series Editor
"This book dives into the tradition and situates itself within academic discussions, but it is written in accessible prose that is always connected to the practical and everyday lives of families. It is certainly appropriate for the educated non-specialist, and will be an invaluable resource for pastors and other Christian leaders ministering to families."—The Way
"Even coming from a rich Catholic background of understanding, her concepts and applications can be used by any denomination. . . . Anyone looking for a refreshing read on family ethics will not be disappointed with Dr. Rubio's work."—Theological Book Review
"Very compelling indeed. . . . Rubio provides the theological community with a new and refreshing way of thinking about the 'goods of marriage.' . . . This book is a 'must read' for anyone who is interested in family life, marriage and relationships in general. It will provoked much discussion in many areas of sexual morality as well as making ordinary married couples feel more at home in theological discussions on sexuality, family and marriage."—The Furrow
"Rubio outlines the thickest and most accessible vision yet of Catholic family life in contemporary America. . . . Rubio's book should gain a very wide audience, for its significance goes beyond advancing the conversation in marriage and family ethics. The book is also suggestive of a way forward for Catholicism generally, one that advances on the lightweight accomodationism of liberals and the unfailing political severity and legalism of conservatives. How so? By an actual retrieval of some of the best work done in early 20th century Catholicism. Instead of simply rejecting anything 'pre-Vatican II' or inventing some supposed set of free-floating pieties that will 'return' us to those days, Rubio actually seeks out the 'nuanced' and 'rigorous analysis' of the literature of that time. In it, she finds both the counterculturalism she desires and the realism she needs. Her work here is suggestive for moral theologians a caricature of the evils of "the moral manuals" may overlook much of value that was unfortunately lost amidst the politics of the postconciliar Church."—Catholic Books Review
"Suitable for undergraduate and graduate students, especially those preparing for ministry, or adult study groups."—Jacob Kohlhaas, Religious Studies Review
"Not simply a Christian 'how-to' manual on the challenge of applying Christian moral teaching to the family; [Rubio] provides a thorough ethical analysis of family life informed by a rich theology of Christian marriage, an astute reading of diverse ethical traditions, and a healthy leavening of wisdom gained from personal experience. Quite simply, with this volume, Julie Hanlon Rubio has now emerged as one of the most insightful theologians today on the theology of marriage and family."—Richard R. Gaillardetz, Murray/Bacik Professor of Catholic Studies, University of Toledo
"A splendid re-visioning of the vocations of marriage and family for Catholics in today's world. Moving past the moral debates of the Vatican II era, Rubio inspires and challenges by linking the social vocation of the family to the concrete practices and problems of family life. She speaks from the realism and wisdom of experience, backed by current data on real-world social trends, relationships, and human behavior. Rubio is that rare voice that manages to keep ideals in clear view while appreciating the importance of negotiation, compromise, persistence, and practicality. This book will satisfy everyone from scholars to students to all those working out connections between faith and family."—Lisa Sowle Cahill, Monan Professor of Theology, Boston College
"Julie Hanlon Rubio has given us the complete book on marriage and family. She weaves Christian theology, worship, and social ethics into the real fabric of everyday life in the home. The scope of the book is impressive. More remarkable, however, is the ease of its integration of Scripture, Catholic social thought, and moral theology in practical matters of sex, food, and money."—David Matzko McCarthy, Fr. James M. Forker Professor of Catholic Social Teaching, Mount St. Mary's University
"Julie Hanlon Rubio's social ethics approach to family moral practices provides long-overdue tools for acting thoughtfully on the competing projects and commitments that absorb most of the waking hours of ordinary people. Graceful and subtle with no loss of prophetic clarity, it will set a new standard for family ethics."—Cristina L. H. Traina, associate professor, Department of Religion, Northwestern University
"An extraordinary and challenging book on the somewhat ordinary topic of family life. In theory Rubio deftly develops a Christian ethics recognizing both grace and sin as a basis for Christian family life. She convincingly shows how five distinct family practices can not only bring about communion in the family but can also serve the common good in solidarity with the poor."—Charles E. Curran, Elizabeth Scurlock University Professor of Human Values, Southern Methodist University
Introduction: Why Be Concerned with the Ordinary?
Part I: Resources from the Catholic Tradition
1. A Catholic Theological Understanding of Marriage
2. Between the Personal and the Political: Families as Agents of Social Change
3. Grace, Sin, and Holy Families
Part II: Practices
4. Practicing Sexual Fidelity
5. The Practice of Eating: Love, Justice, and Mercy
6. How Much Is Enough? The Practice of Tithing
7. Serving: Reimagining a Central Practice of Middle-Class Family Life
8. Family Prayer as Practice of Resistance
Family Transformed
Religion, Values, and Society in American Life
Prophetic and Public
The Social Witness of U.S. Catholicism
A Theological Introduction


Please review and follow these Wilderness regulations to help keep these areas clean, wild, and pristine. FR numbers refer to the Code of Federal Regulations, a collection of rules and regulations enforced by federal agencies.
Entering the Wildernesses without a permit is prohibited (36 CFR 261.57(a)). Permits are required to enter any Wilderness on the Gifford Pinchot National Forest. These permits are free, and may be self-issued at trailheads.
Entering or being in wilderness with a group consisting of a combination of persons, and pack or saddle stock exceeding 12 in total number is prohibited (36 CFR 261.58(f)).
Shortcutting a trail switchback is prohibited (36 CFR 261.55(d)).
Camping within 100 feet slope distance from the shoreline of any lake and/or the Pacific Crest National Scenic Trail, except at Dana Yelverton Shelter within the Goat Rocks Wilderness is prohibited (36 CFR 261.58(e)).
Grazing, hitching, tethering, or hobbling any pack or saddle livestock within 200 feet slope distance of the shoreline of any lake is prohibited (36 CFR 261.57(a), 36 CFR 261.57(e), 36 CFR 261.58(aa)).
Possessing or transporting unprocessed hay or grain livestock feed is prohibited (36 CFR 261.58(t)). Unprocessed hay or grain is defined to mean baled hay or straw or other forms of livestock feed that may serve as a seed source for noxious weeds, non-native, or other undesirable plants.
Caching or storing equipment, personal property, or supplies is prohibited (36 CFR 261.57(f)). Caching is defined to mean leaving equipment unattended for more than 48 hours.
Being in an area posted as being closed for restoration, wilderness restoration, or rehabilitation is prohibited (36 CFR 261.53(b)).
Purchase of a Cascades Volcano Pass is required if you are climbing above 7,000 feet elevation in Mt. Adams Wilderness, between June 1 and September 30. (For Wilderness travel at low elevation or during the off-season, Wilderness Permits are required.) (Failing to pay any special use fee or other charges as required. 36 CFR 261.10(n)).
Building, maintaining, or using a campfire within Shoe Lake Basin, at Dana Yelverton Shelter or within ¼ mile of the shoreline of Goat Lake (36 CFR 261.52(a)) is prohibited.
Camping within Snowgrass Flat and camping within Shoe Lake Basin is prohibited (36 CFR 261.58(e)).
To help visitors comply with wilderness regulations, a designated campsite system has been implemented in the vicinity of Thomas and Blue Lakes.


With a reputation for relative safety and an embarrassment of riches in terms of destinations, Peru is attracting visitors from the United States at record rates.
A report from Peru's government tourism promotor PromPeru noted that 598,685 U.S. travelers came to the country in 2017 to visit. PromPeru's numbers detailed that international arrivals to Peru have grown by five percent each year since 2013.
Peru tourism has been well positioned with the country's rich gastronomy, natural wonders, and an array of adventure options. The country's tourism promotor credited the rise to increased marketing campaigns in the U.S., as they have collaborated with media like the New York Times and Lonely Planet, and coordinated the final of the popular reality series "The Bachelor" to air its finale in Peru.
The bump in visitors saw a record-mark of $4.5 billion invested within Peruvian borders, according to PromPeru. Statistics from the government show that U.S. tourists spend around an average of $1,500 per person during visits to Peru, compared to less than $1,000 for tourists from other countries.
In a recent travel feature, the New York Times recommended Kuélap as one of its "52 Places to Go in 2018," calling it the perfect alternative to Machu Picchu.
¡#MachuPicchu nuevamente se corona como la favorita de miles de viajeros!
Peru World Cup preview: Can La Blanquirroja keep up its hot streak in Russia?


text:
by Young Critic | May 18, 2021 | 0 comments
First feature film from Pilar Palomero beautifully explores the contradictions of modern Spain
Spain has undergone a rapid modernization since its impressive transition from a dictatorship into a democracy in the 1970s. It skyrocketed from a rickety country struggling not to be of the third world, to a first-rate nation of the envy of many. However, in such a rapid and complex transition, Spanish society itself has not been able to keep up, as such there has been a divergence between the sheen that Spain sold to the world, and the actuality lying beneath.
The Girls (2020) is the triumphant winner at the biggest Spanish cinema awards: the Goyas. It made the rounds in film festivals as well, winning an accolade at the San Sebastian Festival, and being included in the Berlin Film Festival's competition lineup. The film follows schoolgirl Celia (Andrea Fandos) in 1990s Spain, with the backdrop of the Barcelona Olympic Games, as she starts to undergo puberty and the beginning of adolescence, which clash with the all-girls catholic school and poverty around her.
This is director Pilar Palomero's first film, and she is quick to demonstrate an impressive hand at restraint and symbolism that never verges on pandering surrealism. Cinematic comments on the Spanish transition and the fake sheen of modernity have been made in the past, by no less than the likes of Luis Buñuel in The Obscure Object of Desire (1977) and Carlos Saura with Blindfolded Eyes (1978). However, those films leaned heavily into symbolism, leaving the suggestive parallel narrative slightly bereft of emotional heft. Saura corrected himself with the more rounded Deprisa, Deprisa (1981), but even then would indulge into the attractions of the crime genre. The Girls is not afraid of taking on a gentle and pensive approach, a decision that might alienate viewers looking for more straightforward entertainment, and yet will reward those who admire filmmaking that staves off noise.
Palomero's framing of her characters' journey as a coming-of-age story is a perfect way to bring forward the deeper messages about the Spanish transition and a confused "young" Spain entering the world arena. However, the whiplash of the decades of dictatorship is still apparent, and it would have been foolish to think that these would simply dissipate with the arrival of democracy. The Girls is able to make curious parallels between Celia finding her femininity, her curious forays into her first cigarette, alcoholic drink, or being asked out by a boy, and the contrasted reactions with her teacher nuns and stressed mother (Natalia de Molina) who resort to a subtle humiliation and imposition of Catholic dogma, and outright lies to tame Celia's newfound curiosities. However, Palomero doesn't resort to over-the-top cruelty or repression to showcase the restrictions and emotional censure that Celia undergoes, rather it is through the more subtle comments, or gestures, which crush and embarrass Celia into keeping her head down.
As with Buñuel and Saura's work, Palomero also focuses her criticisms of modern Spain and the democratic transition on the silence that was imposed around the horrors of the dictatorship period. This was in itself an actual law, "The Pact of Forgetting," where total amnesty was given to all those of Franco's dictatorship, and nothing more was to be said about it. This has carried over, decades later, into a surrounding taboo around the subject of Franco and the dictatorship years (not so much on the Civil War, though that's a whole other essay), which are framed as being best forgotten in the past and not discussed. Palomero makes intriguing parallels with the lies and suppressed truth that Celia is inevitably curious about, and yet when asking about it is quickly shut down with a quick lie and told to never speak of such subjects again. Palomero is incredibly effective with the use of a child's curiosity as a lens to show how it is necessary and natural for Spain to face and discuss its past in order to "grow up."
Celia's troubles and conflicts are perfectly showcased by the fabulous young Fandos, who is a true revelation. It is always hard to get not only acceptable, but watchable performances from any child performer, and yet every once in a while you get a truly adept performer who feels like a veteran. Such was the feeling with Abraham Attah in Beasts of No Nation (2015), and Fandos brings a similar subtlety and internality to her performance. Staying in line with the minimalism and theme of concealing from the film, Fandos' performance is an absolutely perfect fit. The partnership between director and actor works beautifully to channel the confusion and irritation from the continuous suppression and patronizing actions of the adult characters.
The Girls proves to be a fascinating look at the contradictions that modern Spain purports, one of a country putting forward a veneer of great progress, but whose innards are still scarred from the suppressed horrors of the dictatorship. Palomero's ingenious way of framing this subject with a coming-of-age story works to perfection, bringing forth a straightforward and emotionally wholesome narrative, that also works as a parallel exploration for those viewers seeking to dig deeper into the thematic elements. For a first work, to produce a project that can speak on both those levels is truly impressive; to then harness incredible child performances is just showing off. Palomero has come forth onto the cinematic stage with a meditative bang; I can't wait to see what she does next.
What is your favorite debut feature of a director? Comment below


text:
This site uses cookies to ensure you get the best user experience on our website.
OICR WebDev Team
Global Alliance for Genomics & Health (GA4GH)
GENOMICSANDHEALTH.ORG
The Global Alliance for Genomics and Health (GA4GH) was formed to help accelerate the potential of genomic medicine to advance human health. It brings together over 400 leading institutions working in healthcare, research, disease advocacy, life science, and information technology. Our team continues to be a hands-on digital service provider, helping GA4GH reach a global community and advance the formation of important new data-sharing tools and collaborative initiatives.
The Global Alliance project is an important challenge as it is constantly in flux; their Working Groups, Work Products and Demonstration Projects are continually evolving. The GA4GH website and online tools have to successfully reflect these changes in order to ensure that a global audience is informed and aligned.
"It has been great to have the OICR WebDev team design the new GA4GH website. We have very tight timelines and iterative needs. Given our challenges, I'm impressed with WebDev's thorough process and high-quality designs."
Ashley Hobb, Operations Lead
Global Alliance for Genomics & Health
Global Alliance for Genomics & Health is a communication tool that allows the Global Alliance to clearly communicate the status of their organization, members, and work activities with their key audiences. When designing and developing GA4GH operational tools, our primary aim is to keep them simple so that the GA4GH team is as efficient as possible and can focus on more important operations. For the public audience, the news and events blog is a recent addition to the website that provides a less formal means of discussing internal and external work. The blog has not only been a successful way to increase Google rankings, but also provides a central 'meeting ground' for community members to remain informed and motivated to collaborate on data sharing initiatives.
We are currently working on a member engagement campaign that involves building a private portal allowing GA's operational team to recruit new members and approve membership applications. It will also include features for communication amongst the international group and cut down on operational tasks.
webdev.oicr.on.ca
Terms and Conditions Privacy Statement Accessibility Policies Viewing Files
Website by OICR-WEBDEV, hosted by OICR
Funding provided by the Government of Ontario