joint pd controller added. gains hard-coded. needs to read param server