Switch from setting a hard final joint state constraint to a pose constraint. Still need to handle joint constraints though.
Handle joint constraints too.
Fix debug launch file.