Add orocos script and fix bug in cpp file