void PlatformCtrlNode::receiveCmd(const geometry_msgs::Twist& twist)
{
   mutex.lock();
	trajectory_msgs::JointTrajectory traj;
	kin->execInvKin(twist,traj);
	topicPub_DriveCommands.publish(traj);
   mutex.unlock();
}