From be4278356f64141bb1bb8c95ff8f25c0979dbc72 Mon Sep 17 00:00:00 2001 From: AntoineH Date: Wed, 4 Sep 2024 19:27:08 +0200 Subject: [PATCH] Implement open loop velocity control for SetJoints --- src/cmd/controller/getJoints.go | 3 +- src/cmd/controller/serve.go | 89 +++++++++++++++++++++++++++++++-- src/cmd/controller/setJoints.go | 19 +++---- src/cmd/motor/listen.go | 2 + 4 files changed, 99 insertions(+), 14 deletions(-) diff --git a/src/cmd/controller/getJoints.go b/src/cmd/controller/getJoints.go index 67ce55f..aae6e04 100644 --- a/src/cmd/controller/getJoints.go +++ b/src/cmd/controller/getJoints.go @@ -62,5 +62,6 @@ func GetJoints(){ if err != nil { log.Fatalf("could not send: %v", err) } - log.Printf("Angles (°): %v", r.GetAngles()) //TODO: Format with ° + //TODO : Use Logrus + log.Printf("Angles (°): %v", r.GetAngles()) } \ No newline at end of file diff --git a/src/cmd/controller/serve.go b/src/cmd/controller/serve.go index f0cdde8..ee65b65 100644 --- a/src/cmd/controller/serve.go +++ b/src/cmd/controller/serve.go @@ -10,6 +10,8 @@ import ( "log" "time" "net" + "slices" + "math" motor "github.com/AntoineHX/multi-motors-controller/src/cmd/motor" @@ -59,10 +61,89 @@ type server struct { pb.UnimplementedMotorsControllerServer } +//TODO: Coroutine to regulate motor velocities func (s *server) SetJoints(ctx context.Context, in *pb.Angles) (*pb.Angles, error) { - //TODO: use a coroutines to prevent blocking the main thread - log.Printf("Received: %v", in.GetAngles()) - return &pb.Angles{Angles: in.GetAngles()}, nil + //TODO: use a coroutines to prevent blocking the main thread + var tgt_angles = []float64{in.GetAngles()[0], in.GetAngles()[0], in.GetAngles()[0]} //TODO: Replace by in.GetAngles() + + + //TODO: Check error states of motors + //TODO: Check target angles limits + + //Compute velocities + var( + cmd_vel = []float64{} //Commanded velocities + motor_pos = []float64{} //Current motor positions + max_vels = []float64{} //Maximum velocities + traj_times = []float64{} //Trajectory times + s_traj_t float64 //Trajectory time (Synchronized motion) + limit_vel = false //Limit velocity to slowest motor + ) + for i := range tgt_angles { + //TODO: Use a coroutine to avoid blocking the main thread + motor_state := getMotorState(i) //Get current motor state + motor_pos = append(motor_pos, motor_state.Angle) + max_vels = append(max_vels, motor_configs[i].Max_vel) + + //Compute minimum trajectory times + traj_times = append(traj_times, math.Abs(tgt_angles[i]-motor_pos[i])/max_vels[i]) + } + log.Printf("Requested joint positions: %v -> %v", motor_pos, tgt_angles) + // log.Printf("t: %v - %v", slices.Min(traj_times), traj_times) + //Compute maximal velocities + s_traj_t = slices.Min(traj_times) //Minimum trajectory time + for i := range tgt_angles { + cmd_vel = append(cmd_vel, (tgt_angles[i]-motor_pos[i])/s_traj_t) + if math.Abs(cmd_vel[i]) > max_vels[i] { // Velocity needs to be limited (Flag) + limit_vel = true + } + } + // log.Printf("cmd_vel: %v - %v", cmd_vel, limit_vel) + if limit_vel { //Limit velocity to slowest motor + s_traj_t = slices.Max(traj_times) //Trajectory time of slowest motor + for i := range cmd_vel { + cmd_vel[i]= (tgt_angles[i]-motor_pos[i])/s_traj_t + } + } + + //Send command to motors + log.Printf("Requested joints velocities (%v s): %v", s_traj_t, cmd_vel) + for i, vel := range cmd_vel { + setMotorVel(i, vel) + } + + //Stop motors after trajectory time + go stopMotors(time.Duration(float64(time.Second)*s_traj_t)) + + return &pb.Angles{Angles: tgt_angles}, nil +} + +func stopMotors(delay time.Duration) { + timer := time.NewTimer(delay) + <-timer.C //Block until delay is over + for i := range motor_configs { + setMotorVel(i, 0) + } +} + +func setMotorVel(idx int, vel float64){ + //TODO: Only declare client once per motor + // Set up a connection to the server. + var addr = fmt.Sprintf("%s:%d", ip, motor_configs[idx].Port) + conn, err := grpc.NewClient(addr, grpc.WithTransportCredentials(insecure.NewCredentials())) + if err != nil { + log.Fatalf("did not connect: %v", err) + } + defer conn.Close() + c := pb.NewMotorClient(conn) + + // Contact the server. + ctx, cancel := context.WithTimeout(context.Background(), time.Second) + defer cancel() + _, err = c.SetVelocity(ctx, &pb.Velocity{Velocity: vel}) + if err != nil { + log.Fatalf("could not send: %v", err) + } } //TODO: Fix compiling issue with google.protobuf.Empty message @@ -79,7 +160,7 @@ func (s *server) GetJoints(ctx context.Context, in *pb.Empty) (*pb.Angles, error func getMotorState(idx int)(motor.State){ //TODO: Check if motor server is running - //TODO: Only declare once per motor + //TODO: Only declare client once per motor // Set up a connection to the server. var addr = fmt.Sprintf("%s:%d", ip, motor_configs[idx].Port) conn, err := grpc.NewClient(addr, grpc.WithTransportCredentials(insecure.NewCredentials())) diff --git a/src/cmd/controller/setJoints.go b/src/cmd/controller/setJoints.go index b30aa9e..87d873f 100644 --- a/src/cmd/controller/setJoints.go +++ b/src/cmd/controller/setJoints.go @@ -18,10 +18,6 @@ import ( pb "github.com/AntoineHX/multi-motors-controller/src/proto" ) -var( - tgt_angles = []float64{} //TODO: Add to cobra config -) - //Cobra CLI // setJointsCmd represents the setJoints command var setJointsCmd = &cobra.Command{ @@ -29,8 +25,13 @@ var setJointsCmd = &cobra.Command{ Short: "setJoints command descritpion", Long: `setJoints command descritpion`, Run: func(cmd *cobra.Command, args []string) { - fmt.Println("setJoints called with target :", cmd.Flag("vel").Value) - SetJoints() + fmt.Println("setJoints called with target :", cmd.Flag("j").Value) + tgt_angles, err:=cmd.Flags().GetFloat64Slice("j") + if err!= nil { + log.Fatalf("Failed read requested velocities %v", err) + }else{ + SetJoints(tgt_angles) + } }, } @@ -47,12 +48,12 @@ func init() { // is called directly, e.g.: //TODO: Anonymous flag (with anonymous flag group ?) //TODO: Fix input of multiple values as slice - setJointsCmd.Flags().Float64SliceVar(&tgt_angles, "vel", nil, "Target joint values") //or IntSlice ? - setJointsCmd.MarkFlagRequired("vel") + setJointsCmd.Flags().Float64Slice("j", nil, "Target joint values") //or IntSlice ? + setJointsCmd.MarkFlagRequired("j") } //gRPC Client -func SetJoints(){ +func SetJoints(tgt_angles []float64){ // Set up a connection to the server. var addr = fmt.Sprintf("%s:%d", ip, port) //Defined in controller/serve conn, err := grpc.NewClient(addr, grpc.WithTransportCredentials(insecure.NewCredentials())) diff --git a/src/cmd/motor/listen.go b/src/cmd/motor/listen.go index 601e24e..ae14f57 100644 --- a/src/cmd/motor/listen.go +++ b/src/cmd/motor/listen.go @@ -64,6 +64,8 @@ func listen(){ if err != nil { log.Fatalf("could not send: %v", err) } + + //TODO : Use Logrus log.Printf("Motor state: %f° | %f°/s | %s", r.GetAngle(), r.GetVelocity(), r.GetError()) // Wait for a second