Plannification of trajectories is work-in-progress. And force control of one joint is aso work-inprogress